UNIVERSIDAD DE CHILE FACULTAD DE CIENCIAS FÍSICAS Y MATEMÁTICAS DEPARTAMENTO DE INGENIERÍA ELÉCTRICA DISEÑO Y SIMULACIÓN DE UN INSTRUMENTO PARA LA ESTIMACIÓN DE TORQUE DE UN MOTOR PASO A PASO MEMORIA PARA OPTAR AL TÍTULO DE INGENIERO CIVIL ELECTRICISTA RODRIGO ORLANDO BADÍNEZ LARA PROFESOR GUÍA: JORGE OSVALDO ROMO LÓPEZ MIEMBROS DE LA COMISIÓN: HÉCTOR MILER AGUSTO ALEGRÍA JORGE MARCO ANTONIO LÓPEZ HIDALGO SANTIAGO DE CHILE JUNIO 2007 RESUMEN DE LA MEMORIA PARA OPTAR AL TÍTULO DE INGENIERO CIVIL ELECTRICISTA POR: RODRIGO BADÍNEZ LARA FECHA: 02/08/ 2007 PROF. GUÍA: Sr. JORGE ROMO LÓPEZ “DISEÑO Y SIMULACIÓN DE UN INSTRUMENTO PARA LA ESTIMACIÓN DE TORQUE DE UN MOTOR PASO A PASO” El objetivo general del presente trabajo de título, fue diseñar y simular un instrumento para la estimación de torque de un motor paso-a-paso, utilizando sólo mediciones de corrientes y voltajes, con el fin de proveer una herramienta que permita realizar mantenimiento predictivo, a partir de los síntomas que la señal de torque pueda indicar. Para la realización de esta investigación, fue necesario en primer lugar efectuar un análisis respecto a las características de diseño y operación de los motores paso-a-paso, también conocidos como stepper motors. A partir de esto, se obtuvo un modelo que permite simular computacionalmente el comportamiento de este motor. Con el fin de estimar la posición del rotor, necesaria para el cálculo del torque, se realizó una implementación del algoritmo Filtro de Kalman Extendido. Como resultado final, se crearon dos códigos de subprograma en Matlab. El primero de ellos, permite simular el motor en distintas condiciones de operación; el segundo, permite estimar el torque desarrollado por el motor utilizando sólo las corrientes y voltajes de sus fases, cumpliendo así con el objetivo general de esta Memoria. En particular, el instrumento diseñado es capaz de detectar situaciones anómalas de la carga, como por ejemplo cambios bruscos de ésta, que se traducen en estimaciones aceptables de la posición, de la velocidad, de las corrientes y del torque del motor. En efecto, el error en la estimación del torque desarrollado por el motor no supera el 30% en ningún instante y el error en la estimación del torque medio desarrollado es menor a un 10%. Se concluye que se diseñó con éxito un estimador para el torque desarrollado por un motor paso-a-paso de imán permanente, con una precisión aceptable, el cual únicamente utiliza mediciones de voltajes y corrientes de fase. Cabe señalar que el programa realizado sólo permite efectuar estimaciones para motores bifásicos, pero que las ecuaciones encontradas hacen posible el desarrollo nuevos subprogramas para motores con más fases, dejando esto como un desafío para futuras investigaciones. Finalmente, se propone que el estimador diseñado en este trabajo sea llevado a la práctica, desarrollando físicamente un instrumento electrónico que lo implemente. Dedicado a los Pioneros Agradecimientos Este trabajo no podría haberlo concretado sin el apoyo de personas que han sido fundamentales para mí en este proceso. En primer lugar debo agradecer a los profesores que me han formado como ingeniero, en especial al Sr. Jorge Romo, mi profesor guía, quien me apoyó incluso en los momentos cuando no obtenía resultados satisfactorios. También debo mencionar a los profesores Sr. Héctor Agusto y Sr. Jorge López, quienes me ayudaron con las correcciones del presente informe. No puedo dejar de nombrar al Sr. Pedro Mardones, quien amablemente me orientó y ayudó a elegir el tema de memoria, mostrándome la necesidad desarrollar un instrumento como el diseñado. Por supuesto debo agradecer también a mis amigos, quienes me ayudaron a “sobrevivir” en la universidad al brindarme su impagable amistad y una cantidad innumerable de momentos divertidos, entretenidos y chistosos. Imposible no nombrar a Rodolfo (Loli), Roberto (Ñoño), Nikolai (Choko) y David (Chen), como tampoco es posible omitir a los que me han acompañado desde la época del colegio: Juan José y Conrado. Pero la mayor de los agradecimientos se lo debo a mi familia, la cual me ha brindado su apoyo incondicional, tanto económico como moral, durante todos estos años. Pero más importante que eso, mi familia me ha entregado amor y comprensión, tanto en los momentos de felicidad como en los de cansancio y frustración. En especial agradezco a mi madre Jeannette, quien me ha apoyado desde siempre, y siempre ha estado ahí para enseñarme cosas tan fundamentales como a valorar las cosas realmente importantes de la vida. También agradezco a mi padre Orlando, quien también siempre ha estado cuando lo he necesitado, apoyándome en las buenas y en las malas. Gracias especiales le doy a mi alegre hermana menor Natalia, ya que es mi compinche invaluable quien me ha ayudado infinitamente a hacer maldades. Y muchas gracias también a mi incondicional abuelita, quien es el mejor apoyo del mundo y se merece lo mejor. El más especial y afectuoso de los agradecimientos es a la mujer que me ha acompañado día y noche mientras terminaba esta memoria, mi polola Romina. Te quiero mucho. Índice general Índice general i Índice de figuras iii Índice de tablas v Capítulo 1 Introducción 1 1.1. Objetivos Específicos........................................................................................... 2 1.2. Estado del arte...................................................................................................... 2 1.3. Hipótesis de trabajo y metodología...................................................................... 3 1.4. Alcances ............................................................................................................... 3 1.5. Estructura de la memoria ..................................................................................... 4 Capítulo 2 Motores Paso-a-Paso 5 2.1. Características básicas de los motores paso-a-paso ............................................. 5 2.2. Tipos de motores paso-a-paso.............................................................................. 6 2.3. Detalles constructivos de los motores de imán permanente .............................. 12 2.4. Datos de motores en los que se aplicará el estudio ............................................ 15 2.5. Comentarios ....................................................................................................... 16 Capítulo 3 Relación corriente-torque en motores stepper de imán permanente 17 3.1. Modelo del Motor Paso-a-Paso.......................................................................... 18 3.2. Ecuaciones mecánicas y eléctricas para el motor paso-a-paso PM ................... 22 3.3. Ecuaciones correspondientes al motor a simular. .............................................. 22 3.4. Comentarios ....................................................................................................... 23 Capítulo 4 Observadores y Filtro de Kalman 24 4.1. Observadores...................................................................................................... 24 4.2. Filtro de Kalman ................................................................................................ 25 4.2.1. Modelo del proceso a estimar................................................................... 25 4.2.2. Algoritmo discreto del filtro de Kalman................................................... 26 4.2.3. Determinación de los parámetros ............................................................. 27 4.2.4. Inicialización del filtro ............................................................................. 28 4.2.5. Limitaciones del filtro de Kalman ............................................................ 29 4.3. Filtro de Kalman Extendido ............................................................................... 29 4.3.1. Linealización ............................................................................................ 30 4.3.2. Discretización ........................................................................................... 30 i 4.3.3. Algoritmo ................................................................................................. 32 4.3.4. Estimación de parámetros desconocidos .................................................. 32 4.3.5. Comentarios.............................................................................................. 33 4.4. Implementación del EKF para el motor paso-a-paso......................................... 33 4.5. Comentarios ....................................................................................................... 35 Capítulo 5 Simulaciones y Resultados 36 5.1. Simulación motor paso-a-paso........................................................................... 36 5.1.1. Simulación en modo full-step ................................................................... 37 5.1.2. Simulación en modo microstepping ......................................................... 38 5.2. Simulación Filtro de Kalman Extendido con carga conocida............................ 39 5.3. Simulación Filtro de Kalman Extendido con carga desconocida ...................... 41 5.3.1. Simulación asumiendo una carga nominal constante ............................... 41 5.3.2. Simulación utilizando la carga como parámetro a estimar....................... 42 5.4. Comentarios ....................................................................................................... 44 Capítulo 6 Conclusiones 45 6.1. Recapitulación del trabajo realizado .................................................................. 45 6.2. Aspectos positivos y negativos del trabajo ........................................................ 45 6.3. Eventuales aplicaciones del trabajo desarrollado............................................... 45 6.4. Sugerencias para futuras investigaciones sobre el tema .................................... 46 6.5. Comentarios finales............................................................................................ 46 Referencias 47 Anexo A - Códigos Matlab 50 A.1 stepper_rbl.m...................................................................................................... 50 A.2 run_stepper.m..................................................................................................... 50 A.3 torque_carga.m................................................................................................... 53 A.4 genera_voltajes.m .............................................................................................. 53 A.5 kalman_iteracion_v2.m...................................................................................... 56 A.6 ejecución_kalman_codigo.m.............................................................................. 56 A.7 ejecucion_EKF_codigo.m.................................................................................. 60 A.8 ejecucion_EKF2_codigo.m................................................................................ 66 Anexo B - Diagramas Simulink 75 B.1 sistema_stepper_rbl.mdl .................................................................................... 75 ii Índice de figuras Figura 2.1 - Sistema de motor paso-a-paso [14] .....................................................................6 Figura 2.2 - Esquema del motor VR de 3 fases y de sus devanados [20, 21].........................7 Figura 2.3 – Avance de un paso en el motor paso-a-paso VR [20, 21] ..................................7 Figura 2.4 – Movimientos obtenidos al conmutar las 3 fases en el motor VR [20, 21] .........8 Figura 2.5 - Esquema de motor PM con rotor de 2 polos y 2 fases [20, 22] .........................8 Figura 2.6 - Esquema de motor PM con rotor ........................................................................8 Figura 2.7 - Estructura del rotor de un motor híbrido [20, 21] ...............................................9 Figura 2.8 - Estructura de un motor híbrido simple de 12 pasos por revolución [24] ..........10 Figura 2.9 - Recorrido en modo Full-Step [24] ....................................................................11 Figura 2.10 - Paso intermedio en modo Half-Step (entre d y c) [24]....................................11 Figura 2.11 – Esquema de un motor híbrido de 2 fases (4 polos por fase)...........................12 Figura 2.12 – Un motor PM Can-Stack ................................................................................13 Figura 2.13 – Partes principales del motor Can-Stack [24] ..................................................13 Figura 2.14 - Motor PM de 24 polos, construcción real Can-Stack [27]..............................14 Figura 2.15 - Visualización de la interacción dientes del estator con rotor [24] ..................14 Figura 2.16 – Detalle del estator del motor PM Can-stack [27]...........................................15 Figura 3.1 – Motor paso a paso genérico (con P=3 y N=4) [15] ..........................................18 Figura 4.1 – Esquema de un observador...............................................................................25 Figura 4.2 – Diagrama del algoritmo del filtro de Kalman en su versión discreta ...............27 Figura 5.1 - Modelo del motor paso-a-paso de 2 fases en Simulink (anexo B.1) .................37 Figura 5.2 – Respuesta del motor a una secuencia de pulsos ..............................................37 Figura 5.3 – Respuesta del motor a una secuencia de pulsos 2 ...........................................38 Figura 5.4 – Respuesta del motor a entradas sinusoidales (microstepping) .........................38 Figura 5.5 – Respuesta del motor a entradas sinusoidales (microstepping) 2 ......................39 iii Figura 5.6 – Simulación Filtro de Kalman Extendido con carga conocida ..........................40 Figura 5.7 – Corriente de fase medida, estimada y real........................................................41 Figura 5.8 – Simulación asumiendo una carga nominal constante.......................................42 Figura 5.9 – Simulación utilizando la carga como parámetro a estimar...............................43 iv Índice de tablas Tabla 3.1 – Ecuaciones del motor paso-a-paso PM genérico ................................................. 22 Tabla 3.2 – Ecuaciones simplificadas para el motor paso-a-paso PM de 2 fases y 10 polos . 23 Tabla 4.1 – Ecuaciones de predicción del filtro de Kalman discreto...................................... 26 Tabla 4.2 – Ecuaciones de corrección del filtro de Kalman discreto...................................... 26 Tabla 4.3 – Sistema lineal obtenido al aproximar el sistema no-lineal .................................. 30 Tabla 4.4 – Parámetros del sistema discretizado .................................................................... 31 Tabla 4.5 – Parámetros aproximados del sistema discretizado............................................... 31 Tabla 4.6 – Aproximación lineal discretizada de un sistema no-lineal .................................. 32 Tabla 4.7 – Ecuaciones de predicción del filtro de Kalman discreto...................................... 32 Tabla 4.8 – Ecuaciones de corrección del filtro de Kalman discreto...................................... 32 Tabla 4.9 – Formulación en variables de estado del modelo del motor paso-a-paso ............. 33 Tabla 5.1 – Parámetros del motor simulado ........................................................................... 36 Tabla 5.2 – Parámetros simulación filtro de Kalman Extendido ............................................ 39 Tabla 5.3 – Parámetros simulación filtro de Kalman Extendido para carga desconocida...... 42 v Capítulo 1 Introducción En general, un motor cumple con la tarea de efectuar algún movimiento o trabajo mecánico, ya sea de empuje o tracción. Un motor eléctrico es un dispositivo que transforma energía eléctrica en energía mecánica; son altamente eficientes y por ello ampliamente utilizados en instalaciones y procesos industriales. Los motores eléctricos pueden ser de baja o alta potencia y, según sea el caso, cumplir el rol de actuador de potencia (como por ejemplo en compresores, ventiladores, correas transportadoras, prensas, etc.) o bien de actuador de control (control de válvulas, posicionadores de instrumentación, rotación controlada a baja velocidad, etc.) [25]. Los motores, como toda maquinaria, sufren desgaste y deterioro progresivo debido al uso y al paso del tiempo. Más allá de que el deterioro puede ser más rápido o más lento, según como sean las características propias de cada motor y las condiciones de uso, es indispensable aplicar las tareas de mantenimiento para extender así su vida útil. El mantenimiento se puede clasificar como correctivo, preventivo o bien predictivo. El mantenimiento correctivo es el más rudimentario y se basa en intervenir el equipo para repararlo o cambiarlo cuando se produce una falla grave. El mantenimiento preventivo, en cambio, tiene como objetivo evitar eventuales fallas, mediante intervenciones periódicas programadas. Ambos tipos de mantenimiento resultan potencialmente costosos y engorrosos cuando el equipo (motor, en este caso) forma parte de sistemas complejos y de gran tamaño. El mantenimiento predictivo o sintomático está orientado a detectar fallas incipientes, cuando recién se están comenzando a gestar. La falla que se está gestando se detecta observando en línea alguna variable (corriente, velocidad, temperatura, vibraciones, etc.). Mediante un análisis del registro de ésta puede determinarse si existe alguna anomalía que se deba a una falla incipiente. Si es así, habrá tiempo para programar adecuadamente la salida de operación del equipo, para efectuar las reparaciones y ensayos correspondientes. Se evita así que se llegue a una falla franca que detenga el proceso productivo en forma imprevista. La presente Memoria está orientada al mantenimiento que se requiere en los avanzados observatorios astronómicos del norte de Chile, que emplean instrumentos mantenidos a temperaturas criogénicas. El problema específico que aquí se aborda son los motores eléctricos paso-a-paso (o stepper/stepping motors), bifásicos, los que fallan cuando se producen condiciones de alta fricción que alteran el torque del motor, terminando por bloqueando. En el 1 contexto del mantenimiento predictivo, lo ideal sería detectar la aparición de estos torques de roce en sus inicios, para programar adecuadamente su salida de servicio y reparación. En general, para cualquier motor eléctrico, y en especial si son del tipo paso-a-paso, la única forma de realizar un análisis del torque del motor es extrayéndolo físicamente del equipo al que pertenece para enfrentarlo a pruebas directas usando un torquímetro. No existe manera de hacerlo mientras el motor esté en operación, lo cual conlleva a que el tipo de mantenimiento practicable sea preventivo y no predictivo. Considerando lo expuesto, el objetivo de esta Memoria es diseñar un instrumento que permita estimar en tiempo real el torque de un motor paso-a-paso durante su operación, midiendo las corrientes de sus fases y mostrando la forma del torque a partir de los registros de dichas corrientes. Este trabajo tuvo su origen en el Observatorio Europeo Austral, cuyos sistemas requieren de una herramienta como la planteada. 1.1. Objetivos Específicos Los objetivos específicos de este trabajo son los siguientes: • • • Analizar las características de diseño y operación de los motores paso-a-paso, en particular relacionando la corriente con el torque desarrollado por el motor. Desarrollo de un diseño teórico de un instrumento para la estimación de torque en tiempo real para motores paso-a-paso bifásicos, basado en mediciones de corrientes de sus fases de alimentación. Simular el comportamiento del sistema propuesto. 1.2. Estado del arte En el mercado existe una gran variedad de torquímetros capaces de efectuar análisis de torque a motores paso-a-paso. Los más extendidos son transductores de torque por reacción, transductor de torque rotativo y el transductor de torque por LVDT (Linear Variable Differential Transformer). Sin embargo todos ellos requieren, para su utilización, la extracción del motor de su lugar de operación pues necesitan acceso al eje. No existe en el mercado un dispositivo que permita estimar de manera externa el torque de un motor paso-a-paso en operación. En cuanto al desarrollo teórico, existen a la fecha varios artículos de publicaciones científicas que señalan métodos para estimar algunas o todas las variables de estado de un motor paso-apaso (en general sólo para motores de 2 fases), utilizando diversa información de entrada para ello. Los resultados de las estimaciones son de distinta precisión según el método utilizado. En [1] se muestra un algoritmo optimizado del Filtro de Kalman Extendido utilizado para estimar la posición y velocidad del rotor, midiendo las corrientes en cada fase. En [2] los mismos autores muestran para el mismo sistema una versión Steady-State del filtro de Kalman, el cual necesita menos capacidad computacional. Existen también artículos [3-7] que tratan sobre controladores 2 de posición y/o velocidad por retroalimentación, los cuales mejoran sustancialmente el desempeño del motor paso-a-paso comparándolo con el esquema clásico de lazo abierto, y algunos utilizan estimaciones del estado para la retroalimentación, como por ejemplo en [8-9]. Las publicaciones [10-11] tratan sobre como maximizar la eficiencia del motor usando información estimada del torque, y sobre la identificación de parámetros de un motor paso-apaso, respectivamente. Además de las publicaciones anteriores, [12-13] tratan sobre la implementación de torquímetros “sin sensores” para motores de inducción, los cuales usan información de voltajes y corrientes para realizar la estimación. No se encontraron implementaciones similares para motores paso-a-paso. 1.3. Hipótesis de trabajo y metodología La hipótesis de trabajo fundamental fue suponer que utilizando sólo mediciones de la corriente consumida por el motor, es posible obtener una buena estimación del torque. Por ello se tuvo como restricción que el instrumento diseñado debía utilizar como entrada sólo las mediciones de corriente en las fases del motor. La metodología de trabajo fue la siguiente: primero se realizó una investigación teórica sobre los motores paso-a-paso, para conocer sus características y comprender su funcionamiento. Una vez concluida esa etapa, se desarrolló un modelo del motor paso-a-paso, adecuado para las siguientes etapas. Luego se efectuaron simulaciones computacionales del modelo, para un motor de las características deseadas. Posteriormente se realizó una nueva investigación teórica, para encontrar un observador adecuado que permitiera estimar el torque buscado. Finalmente, se simuló el desempeño del observador, para analizar su desempeño. En resumen, se dividió el trabajo en 2 etapas consecutivas: una orientada hacia el motor pasoa-paso y la otra hacia el observador. En ambas se cubrieron los siguientes aspectos: • • • Investigación teórica Desarrollo de un modelo del sistema Validación del diseño utilizando simulaciones computacionales 1.4. Alcances Este trabajo de titulación se centró principalmente en el desarrollo teórico del instrumento. Se limitó el diseño específicamente a un motor paso-a-paso de imán permanente, de 2 fases. Para una implementación más precisa del diseño, se requeriría realizar un adecuado proceso de identificación de los parámetros del motor, tarea que se consideró fuera del alcance del trabajo. 3 1.5. Estructura de la memoria La presente Memoria se ha estructurado dividiéndola en 6 capítulos, que se describen a continuación: • • • • • • El Capítulo 1 entrega un marco general del documento y del trabajo mismo. Se muestra la motivación, objetivos, hipótesis del trabajo, junto con un breve resumen de la metodología utilizada y los alcances que tuvo este trabajo de título. El Capítulo 2 proporciona antecedentes generales sobre los motores paso-a-paso, y antecedentes específicos sobre el tipo de motor que se ha considerado en esta memoria. En el Capítulo 3 se desarrolla un modelo para el motor paso-a-paso, obteniendo así la expresión que relaciona el torque desarrollado por el motor con las corrientes de fase. El Capítulo 4 trata sobre observadores y el filtro de Kalman. Resume sus antecedentes y distintas versiones, junto con sus algoritmos. El Capítulo 5 entrega los resultados de las simulaciones, tanto del modelo del sistema como del observador. El Capítulo 6 resume los resultados obtenidos del trabajo, y muestra las conclusiones obtenidas. 4 Capítulo 2 Motores Paso-a-Paso Los motores paso-a-paso, conocidos también como motores stepping/stepper, son dispositivos electromecánicos que convierten una entrada de pulsos digitales a movimientos discretos e incrementales. Fueron creados a partir de la necesidad de disponer de un dispositivo capaz de producir un desplazamiento angular definido y de mantener su posición frente a un torque aplicado al eje. Se caracterizan por tener la capacidad de proveer posicionamiento preciso, con excelente repetibilidad, sin necesidad de emplear retroalimentación [15, 20]. Con la masificación de la computación y sistemas digitales, y el desarrollo del control digital en el campo del control automático, los motores paso-a-paso han tenido un gran auge reemplazando a los motores de corriente continua en diversas aplicaciones, debido principalmente a su naturaleza “digital” (no necesitan utilizar conversores D/A) [26]. Aplicaciones de este tipo de motores se encuentran por ejemplo en robótica, periféricos computacionales, cámaras digitales, máquinas de control numérico CNC, control de procesos como controladores de válvulas, instrumentos de medición y maquinaria industrial automatizada, entre otras [16, 20]. El presente capítulo entrega una visión general sobre el funcionamiento electromecánico de los motores paso-a-paso, además de una descripción y diferenciación de los distintos tipos de motores existentes. Adicionalmente se incluye información específica sobre los motores de imán permanente utilizados en esta memoria. 2.1. Características básicas de los motores paso-a-paso La característica distintiva de un motor paso-a-paso es, como lo indica su nombre, su capacidad de avanzar “por pasos”. Cada paso de rotación es respuesta a un pulso de entrada y se refiere un incremento angular constante en la posición del rotor. El motor es capaz tanto de mantenerse detenido en cierta posición, avanzar o retroceder un número cualquiera de pasos, como también girar a una velocidad proporcional a la frecuencia de los pulsos de entrada, siempre como una sucesión de avances discretos o pasos. 5 Un sistema de motor paso-a-paso consiste de tres elementos básicos, algunas veces combinados con una interfaz al usuario (como un computador o PLC), como se muestra en la Figura 2.1 [14]. El primer elemento, el controlador, es un microprocesador capaz de generar pulsos y señales de dirección al driver. El driver o amplificador, convierte las señales de pulsos generados por el controlador en el voltaje necesario para energizar los devanados del motor. El último elemento es el motor paso-a-paso en sí. Figura 2.1 - Sistema de motor paso-a-paso [14] En general los motores paso-a-paso son comparativamente de bajo costo, robustos, de rápida aceleración y respuesta. No contienen escobillas, lo que los hace más duraderos en el tiempo y con menor necesidad de mantenimiento con respecto a los motores DC. Su error de posición no es acumulativo si son controlados de manera correcta, esto es sin producir saltos de pasos. Pueden lograr un amplio rango de velocidades modificando la frecuencia de los pulsos. Permiten un control digital de la posición y velocidad [15]. Una de las ventajas más significativas de los motores paso-a-paso con respecto a otros motores eléctricos es su capacidad de poder ser controlado con precisión con un esquema de control de lazo abierto. Un control de lazo abierto implica que no es necesario retroalimentar al controlador con información de la posición, eliminando la necesidad de utilizar sensores de posición [16]. Los problemas típicos que presentan estos motores son resonancia a altas velocidades [17], torque decreciente al aumentar la velocidad, y la existencia de sobreoscilación con un tiempo relativamente lento de asentamiento para los pasos [18]. Muchos de estos problemas se resuelven o reducen al utilizar un esquema de control de lazo cerrado por retroalimentación de la posición, lo cual aumenta considerablemente el rendimiento del motor [5,7] con la desventaja de aumentar bastante el costo del sistema. 2.2. Tipos de motores paso-a-paso Los tres tipos básicos de motores paso-a-paso son el motor de reluctancia variable, el motor de imán permanente y el motor híbrido, que resulta de una combinación de los dos anteriores. En todos ellos, sólo en el estator existen bobinas. El estator está formado por una serie de polos inductores rodeados por sus devanados de excitación, y puede ser de un número variable de fases. El rotor en cambio puede ser de acero laminado, o bien ser un imán de magnetización permanente, según el tipo de motor. 6 Figura 2.2 - Esquema del motor VR de 3 fases y de sus devanados [20, 21] Motor paso-a-paso de reluctancia variable (VR) Este tipo de motor se caracteriza por no poseer imán permanente, en vez de ello su rotor es un bloque dentado de acero o hierro dulce laminado, con salientes y ranuras como se observa en la Figura 2.2. Cuando los devanados del estator son excitados por corrientes, los polos del mismo se magnetizan y atraen al material magnéticamente permeable del rotor. A la vez, esto mismo se puede entender aplicando el principio de minimizar la reluctancia a través de la trayectoria del campo magnético aplicado. El rotor gira hasta encontrar el punto de menor reluctancia, lo que ocurre al alinearse los polos energizados del estator con los más cercanos del rotor. Al alternarse las bobinas energizadas del estator, el campo magnético inducido va girando, produciendo el desplazamiento del rotor como se ilustra en las figuras 2.3 y 2.4 para un motor VR de 3 fases. [14, 26] Figura 2.3 – Avance de un paso en el motor paso-a-paso VR [20, 21] En este tipo de motor, cuando no hay excitación no existe ningún flujo magnético, por lo cual el rotor gira libremente (no existe el llamado torque de detención). Cabe mencionar que en estos motores el número de polos del rotor siempre es menor al del estator, para de esta manera asegurar que sólo un conjunto de polos del estator y rotor se puedan alinear en cualquier instante [19]. Estos motores permiten velocidades de giro mayores que los motores de imanes permanentes, y presentan inercias menores ya que el peso del rotor es menor. Pero tienen un menor rendimiento (salvo para altas velocidades), por lo que son aptos para el movimiento de cargas pequeñas. Además son indiferentes a la polaridad de la corriente, por lo que requieren de una configuración distinta de drivers que el resto de los motores paso-a-paso. 7 Figura 2.4 – Movimientos obtenidos al conmutar las 3 fases en el motor VR [20, 21] Existe una versión del motor de reluctancia variable llamada multistack, en la cual el motor es dividido -según su largo axial- en secciones aisladas magnéticamente, lo que permite obtener ángulos de paso menores que en la versión tradicional. En este tipo de motor, cada stack corresponde a una fase, y se tiene el mismo número de dientes tanto en el estator como en el rotor. Por cada fase, las bobinas de polos contiguos producen magnetizaciones opuestas debido al sentido de las espiras. En el rotor cada stack o sección está desfasada de la siguiente en un ángulo correspondiente al del ancho de un polo dividido por el número de stacks [20]. Motor paso-a-paso de imán permanente (PM) Los motores paso-a-paso de este tipo funcionan según la interacción que se produce entre el rotor, permanentemente magnetizado, y el campo electromagnético creado por las bobinas del estator. A diferencia del motor de reluctancia variable, el rotor acá no presenta dientes salientes, sino que está magnetizado radialmente con un número variable de polos, de polaridades Norte y Sur alternadas a lo largo de la circunferencia del rotor [14-16]. Las Figuras 2.5 y 2.6 muestran esquemas motores de magnetización permanente de 2 y 4 polos respectivamente, ambas de dos fases. Figura 2.5 - Esquema de motor PM con rotor de 2 polos y 2 fases [20, 22] Figura 2.6 - Esquema de motor PM con rotor de 6 polos y 2 fases [16] El flujo magnético inducido por el estator interactúa con el flujo magnético proveniente del imán permanente del rotor. Cuando ambos flujos están alineados en el mismo sentido, el motor está en un punto de equilibrio estable y no hay torque producido. Cuando los flujos están alineados pero con sentidos opuestos, el rotor está en un punto de equilibrio inestable sin torque 8 [19]. Cualquier otra posición angular produce torque en el rotor que tiende a llevarlo al punto de equilibrio estable; el máximo torque se produce al estar los flujos magnéticos perpendiculares entre sí. El motor paso-a-paso de imán permanente es el tipo más extendido en aplicaciones no industriales, pues es el que tiene menor costo de producción. Tiene como ventaja frente a los motores de reluctancia variable, que cuando no está excitado, debido a la atracción entre el imán permanente y el estator, mantiene un torque residual de detención; esto evita que el motor gire libremente al estar desenergizado. La magnetización del rotor mejora la característica de torque de este motor, si se compara con el motor de reluctancia variable. Pese a ello, en la misma comparación, el torque disminuye más abruptamente al aumentar la velocidad [16], por lo que los motores de imán permanente se utilizan esencialmente para bajas velocidades (bajo 1000 pasos por segundo). Otra característica importante de estos motores es que, a diferencia de los paso-a-paso de reluctancia variable, son sensibles a la polaridad de la corriente de excitación. Esto implica que necesariamente los drivers para este tipo de motores son más elaborados, puesto que requieren manejar las dos direcciones de corriente en cada una de las fases. Los motores paso-a-paso de magnetización permanente presentan el inconveniente que, como es difícil lograr una magnetización radial con más de 50 polos [16], sus pasos angulares son generalmente grandes, típicamente en el rango de 30º a 7,5º. Por ello son motores de baja resolución angular. Motor paso-a-paso Híbrido El motor híbrido comparte los principios de operación de los motores paso-a-paso de magnetización permanente y los de reluctancia variable. Su rotor es de acero laminado dentado, como en los motores de reluctancia variable, pero su núcleo es un imán permanente concéntrico al eje del motor, magnetizado axialmente con solamente dos polos [20]. Además, el rotor está dividido en dos secciones separadas, ambas desfasadas angularmente entre sí por la mitad del ángulo entre “dientes”, como se muestra en la Figura 2.7. Por la disposición de las secciones, cada una de ella toma una polaridad magnética distinta. Figura 2.7 - Estructura del rotor de un motor híbrido [20, 21] 9 Los dientes del rotor se utilizan para dirigir al flujo magnético, aumentando la eficiencia del motor. Para ilustrar de mejor manera el principio de operación de este tipo de motores, a continuación se detalla el funcionamiento de un motor híbrido simple[24]. Figura 2.8 - Estructura de un motor híbrido simple de 12 pasos por revolución [24] • • • • • • En este caso el rotor tiene 3 salientes por cada sección, y el estator posee 4 polos a lo largo de la extensión del rotor completo (Figura 2.8). Al energizar una de las bobinas, se producen dos polos Norte y Sur los cuales atraen a los salientes del rotor de polaridad opuesta (Figura 2.9.a). Notar que como las secciones del rotor están desfasadas por la mitad del ángulo de separación entre dientes, el resultado neto es análogo a tener un rotor magnetizado radialmente con polos Norte y Sur intercalados. En esta situación sólo hay 3 posiciones estables para el motor, igual a la cantidad de dientes del rotor. Si se cambia la excitación a la otra fase, ahora el campo generado por el estator esta girado en 90º con respecto el anterior. Como resultado de esto se tiene que el rotor gira 30º (Figura 2.9.b). Alternando de ésta manera la excitación de las bobinas (invirtiendo ahora la polaridad de las corrientes por las bobinas), se vuelve a una situación análoga a la original, avanzando 90º (Figuras 2.9.c y d). 10 Figura 2.9 - Recorrido en modo Full-Step [24] • Si entre los pasos anteriores, se energizan las dos bobinas al mismo tiempo, se obtiene el modo Half-step en el cual el motor avanza en medios pasos angulares como se observa en la Figura 2.10. Este modo de operación es aplicable en todos los motores paso-a-paso Figura 2.10 - Paso intermedio en modo Half-Step (entre d y c) [24] Los motores híbridos reales tienen un número mucho mayor de dientes en el rotor, típicamente 50 por sección, y pueden ser de 2 a 5 fases, lo cual permite alcanzar resoluciones angulares en el esquema normal, de 1.8º hasta 0.72º por paso respectivamente. El estator de estos motores tiene una construcción de polos salientes dentados, como se muestra en la Figura 2.11. Sus fases generalmente agrupan varios polos, y utilizan ambas polaridades de corriente. Por esto último utilizan los mismos drivers que los motores de magnetización permanente. 11 Figura 2.11 – Esquema de un motor híbrido de 2 fases (4 polos por fase) La presencia la magnetización permanente en el rotor aumenta el torque de detención (detent), de contención (holding) y mejora las características dinámicas, si se compara el motor híbrido con los otros tipos de motores paso-a-paso. Estas características, junto a su gran resolución (típicamente desde 3.6º a 0.72º), hacen que sea el motor paso-a-paso más usado en aplicaciones comerciales. Sin embargo, son por mucho los que tienen mayor costo, debido a su compleja construcción. 2.3. Detalles constructivos de los motores de imán permanente Considerando que en la aplicación de esta Memoria se utiliza un motor del tipo imán permanente, se entregan a continuación detalles constructivos del mismo. La construcción real de un motor de magnetización permanente es considerablemente diferente a los esquemas mostrados en las Figuras 2.5 y 2.6 de la sección anterior, sobretodo en cuanto al estator. Con el fin de obtener un motor económico con la mayor resolución angular posible, se utiliza el estilo de construcción llamado Can-Stack (en español se puede traducir como “Pila de Tarros”) [27]. En la Figura 2.12 se muestra un motor de este tipo desensamblado, y en la Figura 2.13 un diagrama detallando sus componentes. 12 Figura 2.12 – Un motor PM Can-Stack Figura 2.13 – Partes principales del motor Can-Stack [24] Las bobinas de cada fase están envueltas por una cáscara de acero con forma de rosquilla, con “dientes” o “dedos” salientes apuntando hacia su centro, como se observa en la Figura 2.14. Cuando una bobina es energizada, magnetiza los extremos superior e inferior con polaridades opuestas. Los dientes de cada extremo están dispuestos de manera intercalada. Si el rotor tuviera, por ejemplo 24 polos magnéticos, cada extremo de la “rosquilla” tendría 12 dientes para así tener la misma cantidad de polos por fases que el rotor. La Figura 2.15 muestra como estos dientes magnetizados interactúan con el rotor permanentemente magnetizado [27]. 13 Figura 2.14 - Motor PM de 24 polos, construcción real Can-Stack [27] Figura 2.15 - Visualización de la interacción dientes del estator con rotor [24] Si en nuestro ejemplo de motor se invirtiera la polaridad de esa fase, el rotor se desplazaría 360º/25=15º, pero sin tener una dirección de giro predefinida. Sin embargo, si se energiza primero el devanado φ-1 seguido por φ-2, el rotor se moverá 7.5º ya que por construcción φ-1 está desplazado 7.5º de φ-2, pues esa es la separación equivalente a 90º eléctricos en este motor, como se ilustra en la Figura 2.16 [27]. 14 Figura 2.16 – Detalle del estator del motor PM Can-stack [27] En resumen, el esquema de construcción Can-stack permite obtener una gran cantidad de polos en el estator a partir de una sola bobina, y por ser de una construcción muy sencilla resulta de bajo costo. 2.4. Datos de motores en los que se aplicará el estudio Los motores paso-a-paso que se utilizaron como base para el estudio, son los que se encuentran en las disqueteras de 3.5’’. La función de estos motores es posicionar el cabezal de lectura/escritura magnética; para ello tienen incorporado un tornillo de avance para poder actuar como posicionadores lineales. Pese a la carencia de su documentación oficial, debido a que estos motores son desarrollados a pedido por las empresas, teniendo así diseños propietarios, se lograron recopilar las siguientes características generales: • • • • • Son motores paso-a-paso de imán permanente, de construcción Can-Stack Son del tipo bipolar, es decir, la corriente circula por ambas direcciones en las bobinas, según lo indique el driver. Poseen 2 fases, y 2 bobinas en el estator (4 cables de salida al exterior) Son de 5 Volts, y los valores comunes para las bobinas son: resistencias de 20 ohms, inductancias de 30mH, y corriente nominal típica de 300mA por fase. Cada paso fundamental es de 18º, por lo cual se tienen 20 pasos por revolución y 5 pares de polos magnéticos en el rotor. Cabe mencionar que los motores empleados en los instrumentos astronómicos del Observatorio Europeo Austral (ESO), que motivaron la presente memoria, son del mismo tipo que los recién especificados. Debido a la imposibilidad de contar con los datos de diseño de los motores de ésta organización, se decidió trabajar con los motores de las disqueteras antes señaladas ya que comparten el mismo principio de funcionamiento. 15 2.5. Comentarios Los motores paso-a-paso son actuadores digitales, capaces de proveer posicionamiento y movimiento preciso en cantidades discretas. Requieren de un driver para su utilización, y existen 3 tipos básicos: El motor paso-a-paso de reluctancia variable, el de imán permanente y el híbrido. Los más utilizados son estos dos 2 últimos, y el híbrido es el que presenta las mejores prestaciones en cuanto a resolución y torque. Sin embargo el motor de imán permanente se destaca por su bajo costo, debido a su esquema de construcción llamado Can-Stack, y esto hace que sea el más empleado en aplicaciones no industriales. Los motores paso-a-paso utilizados para el presente trabajo corresponden a este tipo. El próximo capítulo ahonda más en la teoría de este tipo de motores, para encontrar la relación entre corriente de fase y torque del motor, en la que se basó el instrumento desarrollado. 16 Capítulo 3 Relación corriente-torque en motores stepper de imán permanente Los motores eléctricos son máquinas electromagnéticas y electromecánicas, en el sentido de que utilizan relaciones electromagnéticas para transformar energía eléctrica a energía mecánica. El torque desarrollado en los diversos tipos de motores eléctricos es producto de la interacción de un campo magnético rotatorio, creado por el estator, con un segundo elemento derivado de rotor, el cual puede ser un imán permanente, material ferromagnético permeable o un segundo campo magnético inducido. En cualquiera de los casos, el rotor buscará seguir el movimiento del campo magnético rotatorio creado por el estator1. La magnitud del torque generado es proporcional a la intensidad de los campos magnéticos en interacción, y esta intensidad es a su vez función de la magnitud de las corrientes por las fases del motor. Esto ocurre en todos los motores eléctricos. En los que emplean rotor de imán permanente, interactúan dos campos magnéticos, uno de ellos es generado por el imán permanente por lo cual resulta de intensidad constante, y el otro resulta inducido por circulación de corriente en bobinas, de acuerdo con la Ley de Ampère. En los motores en los cuales la interacción del campo se produce con algún material ferromagnético permeable, la generación del campo magnético rotatorio también es consecuencia de circulación de corrientes en el estator. De esta manera, en ambos casos, el torque desarrollado es una función de la corriente consumida por el estator. Además de ser dependiente de la intensidad de los campos magnéticos, la intensidad del torque desarrollado también depende de la posición de los elementos interactuantes. Cuando dos campos magnéticos rotatorios interaccionan se genera un torque que intenta alinearlos, que resulta función de su desfase angular, teniendo su mayor fuerza cuando éste es de 90 grados. Cuando un campo magnético rotatorio interactúa con un material ferromagnético, éste resulta atraído con una fuerza que también resulta dependiente de sus posiciones relativas, ya que sigue el principio de minimizar la reluctancia variable del circuito magnético equivalente. 1 Salvo para los motores de corriente continua (C.C.), donde el torque es producido por la interacción entre un campo magnético fijo generado por el estator, y otro rotatorio inducido producido por el rotor. 17 En este capítulo se analiza el modelo físico del motor paso-a-paso, en particular del tipo imán permanente bifásico, para con él encontrar la relación adecuada entre torque, corriente y posición que se utilizará como base para el diseño el instrumento. 3.1. Modelo del Motor Paso-a-Paso La idea básica de funcionamiento de un motor paso-a-paso es similar a la de un motor síncrono, en donde existe un campo magnético rotatorio generado por las bobinas del estator, que se desplaza mediante conmutación de sus fases. A pesar de estas similitudes, el motor paso-apaso tiene la ventaja de poder rotar al campo magnético a distintas velocidades e incluso poder mantenerlo en alguna posición, según como sea la conmutación de sus fases. Un buen modelo de un motor paso-a-paso debe considerar tres aspectos fundamentales: su comportamiento mecánico, su comportamiento eléctrico, y la relación que existe entre ellos. Consideremos un motor paso-a-paso genérico, como el que se muestra en la Figura 3.1. Figura 3.1 – Motor paso a paso genérico (con P=3 y N=4) [15] Denominando P al número de pares de polos del rotor, y N al número de fases del estator. Designando por θ al ángulo de referencia tomado a partir de la alineación del polo de la fase I con el polo A del rotor, y con sentido creciente según la convención “regla de la mano derecha”. Las ecuaciones que rigen el comportamiento mecánico del sistema son las siguientes: J⋅ dω = Tm − (TC + TV + TF ) dt dθ ω= dt 18 (3.1) (3.2) La ecuación (3.1) es el balance de torque: J es el momento de inercia del sistema (motor más su carga); la velocidad angular es ω; el par de giro o torque desarrollado por el motor es Tm; el torque producido por la carga en el eje del motor es Tc, y los torques de fricción viscosa y estática son Tv y Tf. La ecuación (3.2) es la definición de velocidad angular. Usualmente el roce viscoso se modela proporcional a la velocidad, como lo muestra la ecuación (3.3). TV = B ⋅ ω (3.3) Las ecuaciones eléctricas del sistema son las siguientes[7]: V j (t ) = i j (t ) ⋅ R j + dφ j dt dφ j dt (i1 , K , i N , θ ) ∂φ j ∂ik ∂φ j ⋅ + ⋅ω ∂t ∂θ k =1 ∂ik j = 1, K , N (3.4) N =∑ (3.5) En las ecuaciones anteriores j denota a la fase: Vj son los voltajes, Ij las corrientes, Rj las resistencias de cada fase. El flujo magnético enlazado por las bobinas de la fase j se denota por φj. La ecuación (3.4) corresponde a los circuitos de cada fase. La ecuación (3.5) es el desarrollo de la derivada total. En estos motores es común despreciar las inductancias mutuas entre las bobinas del estator, ya que por consecuencia del diseño su efecto es despreciable si se compara con el de las autoinductancias[7][21]. De esta manera al reemplazar en (3.5) la definición de auto-inductancia y considerando (3.6), se obtiene la ecuación (3.7). M ik = dφ j dt ∂φ j ∂ik =0 = Lj ⋅ para j ≠ k ∂i j ∂t + ∂φ j ∂θ ⋅ω (3.6) (3.7) El torque desarrollado por el motor se puede obtener a partir de la expresión (3.8), en la cual W’ es la llamada co-energía magnética almacenada por el sistema [7]. Tm = ∂W ′ ∂θ 19 (3.8) La definición de co-energía magnética es la expresión (3.9) [7]. Sin embargo, como los motores paso a paso están diseñados para trabajar en la zona lineal de la relación corriente–flujo magnético enlazado, es posible despreciar los efectos de saturación magnética. En esta situación, es posible demostrar que la co-energía magnética y la energía magnética almacenada resultan tener el mismo valor. Las ecuaciones (3.10), (3.11) y (3.12) son distintas expresiones para la energía magnética almacenada[28], designada por W. W′= N ij ∑ ∫φ j =1 0 N φj j =1 0 j ( i1 ' , K , i N ' , θ ) ⋅ di j ' W = ∑ ∫ i j (φ1 ' ,K, φ N ' ,θ ) ⋅ dφ j ' W= 1 N 1 N N 2 L ⋅ i + M ij2 ⋅ ii ⋅ i j ∑ i i 2 ∑∑ 2 i=1 i =1 j >i r v 1 W = i T ⋅ [L]⋅ i 2 (3.9) (3.10) (3.11) (3.12) Así, al igualar (3.9) con (3.12) y reemplazarlo en (3.8) se obtiene la expresión (3.13) la cual relaciona las corrientes de fase, con la posición y las componentes de la matriz de inductancia L del motor[3]. 1 v ∂[L ] r Tm (i1 , K , i N , θ ) = i T ⋅ ⋅i (3.13) 2 ∂θ Caso particular: motor de reluctancia variable En estos motores, es usual asumir la dependencia del flujo magnético enlazado con respecto a la posición como una función sinusoidal más una constante. Así se obtienen mínimos de reluctancia cuando los polos salientes del rotor se alinean con los polos del estator de alguna de las fases, y valores mayores para el resto de las posiciones. La ecuación (3.14) expresa lo anterior, para el flujo magnético φj enlazado por las bobinas de la fase j, con L0 y L1 constantes del motor. φ j = {L0 − L1 ⋅ cos 2 P ⋅ θ − ( j − 1) ⋅ 2π } ⋅ i j N (3.14) Lo anterior, al usarse junto con las ecuaciones (3.8) y (3.9), permite obtener la siguiente expresión para el torque en función de las corrientes de fase y la posición[7]: N ( j − 1) ⋅ 2π Tm = ( P ⋅ L1 ) ⋅ ∑ i 2j ⋅ sin 2 P ⋅ θ − N j =1 (3.15) Caso particular: motor de imán permanente En estos motores se tiene que, por la simetría del rotor, las auto-inductancias de todas las fases tienen el mismo valor, independiente de la posición del rotor [3][21]. 20 ∂φ j ∂i j = Lj = L (3.16) Lo anterior reemplazado en (3.7) y (3.4) permite despejar la derivada de la corriente: di j dt = ∂φ 1 ⋅ V j (t ) − i j (t ) ⋅ R j − ω ⋅ j L ∂θ j = 1, K , N (3.17) Por otra parte, el imán permanente del rotor se puede modelar utilizando una corriente ficticia if del rotor, de magnitud constante [3][7]. Se asume una dependencia sinusoidal flujo enlazado por las bobinas producido por el rotor, con respecto a la posición, como se muestra en la ecuación (3.18) donde Lm0 y Lm1 son parámetros constantes del modelo. φ j = L ⋅ i j + {Lm 0 + Lm1 ⋅ cos P ⋅ θ − ( j − 1) ⋅ π } ⋅ i f N j = 1, K , N (3.18) Con lo anterior, se puede calcular la derivada del flujo magnético con respecto a la posición, la cual al reemplazar en la ecuación (3.17) se obtiene la ecuación diferencial (3.19) para el comportamiento eléctrico, donde km es un parámetro del motor dado por (3.20): di j dt = 1 ( j − 1) ⋅ π ⋅ V j (t ) − i j (t ) ⋅ R j + ω ⋅ K m ⋅ sin P ⋅ θ − L N j = 1, K , N K m = i f ⋅ Lm1 ⋅ P (3.19) (3.20) La ecuación (3.18) muestra que pese a que las inductancias mutuas entre las fases son nulas, la inductancia mutua entre una fase y la bobina ficticia del rotor no lo es[3]. Al incluir el concepto de corriente y bobina ficticia en el sistema para modelar el imán permanente, es deseable también considerar el efecto de la auto-inductancia de la bobina ficticia. Para hacerlo, se puede utilizar la ecuación (3.21) como expresión para la auto-inductancia ficticia Lff, donde Lf0 y Lf4 son constantes[3]. L ff = L f 0 + L f 4 ⋅ cos(4 ⋅ P ⋅ θ ) (3.21) Para incluir lo anterior, se puede generar la matriz de inductancia L extendida, simétrica y de dimensión N+1, que considera a las inductancias ficticias del rotor. Las inductancias mutuas Ljf de la bobina ficticia se obtienen directamente de la ecuación (3.18). La ecuación (3.23) muestra la forma de la matriz de inductancia. ( j − 1) ⋅ π L jf = Lm 0 + Lm1 ⋅ cos P ⋅ θ − N 21 j = 1, K , N (3.22) L 0 [ L] = 0 L1 f 0 0 O 0 0 L L LNf L1 f M LNf L ff (3.23) Finalmente para encontrar la expresión del torque desarrollado, se reemplaza las ecuaciones (3.21) y (3.22) en (3.23), y ésta a su vez en (3.13). Se obtiene así la ecuación (3.24)[7], la cual es una expresión para el torque en función de las corrientes de fase y de la posición angular, donde kd es un parámetro definido por (3.25), que representa al torque de detención debido a la interacción entre el rotor de imán permanente y el material ferromagnético de los polos del estator[9]: N ( j − 1) ⋅ π Tm = − K m ⋅ ∑ i j ⋅ sin P ⋅ θ − − K d ⋅ sin(4 ⋅ P ⋅ θ ) N j =1 (3.24) K d = 2 ⋅ L f 4 ⋅ P ⋅ i 2f (3.25) 3.2. Ecuaciones mecánicas y eléctricas para el motor pasoa-paso PM En resumen, las ecuaciones necesarias para modelar un motor paso a paso de imán permanente son las siguientes: Tabla 3.1 – Ecuaciones del motor paso-a-paso PM genérico dω = Tm − (TC + TV + TF ) dt dθ ω= dt di j 1 ( j − 1) ⋅ π = ⋅ V j (t ) − i j (t ) ⋅ R j + ω ⋅ K m ⋅ sin( P ⋅ θ − ) j = 1, K , N dt L N N ( j − 1) ⋅ π Tm = − K m ⋅ ∑ i j ⋅ sin P ⋅ θ − − K d ⋅ sin( 4 ⋅ P ⋅ θ ) N j =1 J⋅ (3.1) (3.2) (3.19) (3.24) 3.3. Ecuaciones correspondientes al motor a simular. En el caso del motor a simular, el número de fases es 2, y el número de pares de polos del rotor es 5, por lo que las ecuaciones resultan: 22 Tabla 3.2 – Ecuaciones simplificadas para el motor paso-a-paso PM de 2 fases y 10 polos J⋅ dω = Tm − (TC + TV + TF ) dt dθ ω= dt di1 1 = ⋅ (V1 (t ) − i1 (t ) ⋅ R + ω ⋅ K m ⋅ sin(5θ ) ) dt L di2 1 = ⋅ (V2 (t ) − i2 (t ) ⋅ R − ω ⋅ K m ⋅ cos(5θ ) ) dt L Tm = − K m ⋅ i1 ⋅ sin(5θ ) + K m ⋅ i2 ⋅ cos(5θ ) − K d ⋅ sin( 20θ ) (3.1) (3.2) (3.26) (3.27) (3.28) La ecuación (3.28) es la relación corriente-torque buscada. 3.4. Comentarios Según se observa de las ecuación (3.28) del modelo obtenido, la relación corriente-torque resulta dependiente de la posición del rotor. Esto contraviene la hipótesis fundamental asumida para el trabajo, que sostenía que utilizando sólo mediciones de corriente se podía realizar una buena estimación del torque. Sin embargo, como el propósito original del trabajo era proveer un método de estimación de torque, que no necesitara un acceso directo al motor, o la extracción del mismo de su lugar de operación, resulta conveniente relajar esta hipótesis. Resultaría tentador utilizar directamente un sensor de posición, pero esto necesariamente implicaría un acceso al eje del motor, situación que se desea evitar. La solución adecuada entonces, consiste en medir otras variables de acceso externo al motor, que permitan junto con las mediciones de corriente “observar” la posición del rotor, para con ella poder finalmente estimar el torque. Las únicas variables disponibles con estas características resultan ser los voltajes de fase, y por ello el siguiente capítulo trata sobre como usarlos para estimar la posición del rotor. 23 Capítulo 4 Observadores y Filtro de Kalman Un observador se refiere a un algoritmo capaz de producir una estimación (observar) de una variable de estado no-medible de un sistema. Un estado no-medible o no-disponible es una variable de sistema que resulta engorrosa, económicamente costosa, o físicamente inaccesible de medir. Existen básicamente dos tipos de estimadores de variables de estados: los noretroalimentados y los retroalimentados. La diferencia entre ellos es que los segundos poseen un término de corrección, relacionado con el error de estimación, que ajusta la respuesta del estimador. Esto mejora considerablemente el rendimiento del estimador, pues evita que el error de estimación crezca y diverja con el tiempo, como generalmente sucede en los estimadores noretroalimentados. La retroalimentación se refiere a que el estimador utiliza como entrada las salidas del sistema, que sí son variables medibles. A los estimadores con retroalimentación de medición se les denomina observadores. Este capítulo entrega una pequeña introducción sobre los observadores, para luego profundizar en el filtro de Kalman como el observador idóneo para estimar la posición y el torque desarrollado del motor paso-a-paso. 4.1. Observadores Como se menciona en la introducción anterior, los observadores utilizan las mediciones de la salida de la planta más un modelo del sistema, para producir sus estimaciones. Comparan la salida real de la planta con la salida estimada con el modelo, y retroalimentan el error en la estimación para corregir su desviación. La Figura 4.1 muestra esto en un esquema del sistema real con observador. Los observadores se pueden clasificar de acuerdo a la representación utilizada para la planta que se desea observar. Esto significa que el observador puede ser determinístico o estocástico, según la forma como esté descrita la planta. Los observadores más extendidos para ambos tipos de representaciones son el observador de Luenberger y el filtro de Kalman, respectivamente. 24 Figura 4.1 – Esquema de un observador Nos centraremos en el estudio del filtro de Kalman, pues tiene una serie de ventajas para su implementación experimental. 4.2. Filtro de Kalman El filtro de Kalman es un observador estocástico de naturaleza recursiva, que resuelve el problema de estimar el estado de un sistema dinámico lineal, a partir de mediciones contaminadas con ruido blanco. Es un algoritmo ampliamente utilizado desde que en 1960 Rudolph E. Kalman demostró que es un estimador estadísticamente óptimo, en el sentido de que minimiza el error cuadrático de estimación para un sistema lineal. Es decir, no es posible encontrar una mejor estimación de los estados desconocidos[29]. Desde un punto de vista matemático, el filtro de Kalman es un conjunto de ecuaciones que entregan una solución recursiva eficiente para el método de mínimos cuadrados. El filtro de Kalman permite realizar estimaciones del estado, incluso cuando la naturaleza exacta del sistema modelado no es conocida a cabalidad[30]. Es un procedimiento extremadamente flexible y versátil que permite emplear salidas ruidosas de sensores para realizar estimaciones del estado de sistemas con dinámicas inciertas, con parámetros no completamente determinados[14]. Sin embargo tiene la gran limitación de que, en su formulación tradicional, sólo es aplicable para sistemas dinámicos lineales. A continuación se detallará la versión en tiempo discreto del filtro, pues es la más adecuada para una ulterior implementación del algoritmo en un PIC o DSP, para el desarrollo físico del instrumento. 4.2.1. Modelo del proceso a estimar El filtro de Kalman utiliza un modelo del proceso o sistema a analizar, basado en la formulación tradicional en variables de estado para sistemas lineales, pero al que se le agregan fuentes de ruido blanco estocásticas de distribución normal, tanto en el proceso mismo como en proceso de medición, tal como se muestra en las ecuaciones (4.1) y (4.2). En ellas xk representa al estado del sistema, uk la entrada del sistema, yk las mediciones del sistema, wk el ruido en el 25 proceso, y vk el ruido de las mediciones[30]. Los parámetros del modelo son Ak, Bk, Ck, y k es la variable temporal discreta. x k = Ak −1 x k −1 + Bk −1uk −1 + w k −1 (4.1) yk = C k xk + v k (4.2) Las señales aleatorias wk y vk se consideran independientes entre sí, y siguen distribuciones de probabilidad normales con las siguientes características, donde Qk es la covarianza de ruido en el proceso y Rk la covarianza de ruido en la medición: w k : N (0 ,Qk ) (4.3) v k : N (0 , Rk ) (4.4) Aunque los parámetros Ak, Bk, Ck, Qk, y Rk podrían ser funciones del tiempo, por simplicidad muchas veces se les considera constantes en la implementación del filtro. 4.2.2. Algoritmo discreto del filtro de Kalman Este algoritmo se caracteriza por ser de naturaleza recursiva, y por ello sólo necesita la estimación inmediatamente anterior y las mediciones actuales para calcular una nueva estimación de las variables de estado. Consiste básicamente en dos etapas: en la primera se realiza una predicción del estado utilizando la estimación anterior al tiempo actual, y otra en que se corrige la estimación utilizando las mediciones actuales para la salida de la planta[30]. Las tablas 4.1 y 4.2 muestran las ecuaciones de cada fase del algoritmo[29] y la Figura 4.2 resume el algoritmo completo en un diagrama ilustrativo. Tabla 4.1 – Ecuaciones de predicción del filtro de Kalman discreto xˆ ′k = Ak −1 xˆ k −1 + Bk −1uk −1 (4.5) Pk′ = Ak −1 Pk −1 AkT−1 + Bk −1Qk −1 BkT−1 (4.6) Tabla 4.2 – Ecuaciones de corrección del filtro de Kalman discreto S k = C k Pk′C kT + Rk (4.7) K k = Pk′CTk S −k 1 (4.8) xˆ k = xˆ ′k + K k ( yk − C k xˆ ′k ) (4.9) Pk = Pk′ − K k S k K kT (4.10) 26 En las ecuaciones anteriores, es la estimación a priori de las variables de estado para el instante k, y es la estimación final. Se denota Pk a matriz de covarianza del error en la estimación, que es una medida estimada de la precisión en la estimación del estado para el instante k; P'k es la estimación a priori de la misma matriz. Sk es la covarianza de innovación, que se utiliza para calcular la ganancia de Kalman Kk. Figura 4.2 – Diagrama del algoritmo del filtro de Kalman en su versión discreta 4.2.3. Determinación de los parámetros Uno de los aspectos que hacen al filtro de Kalman una elección natural en muchas aplicaciones prácticas, es el hecho de que maneja las fuentes de ruido de una manera natural y certera. Las fuentes de ruido se consideran como variables estocásticas de media cero, y en el algoritmo se manejan con las matrices de covarianza Qk y Rk. Por simplicidad, muchas veces se les consideran constantes en el tiempo, y pasan a llamarse Q y R. El filtro de Kalman posee 5 parámetros en total, y estos son Ak, Bk, Ck, Q y R. Los tres primeros generalmente se derivan directamente de la modelación clásica determinística del sistema. Pero esto no es posible para los parámetros Q y R, que son de obtención más engorrosa. La matriz de covarianza R indica cuán confiables son las mediciones. Si hay demasiado ruido en las mediciones, R es grande e indica que el filtro no debería basar la actualización del estado demasiado en las mediciones, sino basarse más en la estimación del estado anterior. En el caso opuesto R es pequeño, lo cual significa mediciones precisas, por lo cual pueden ser utilizadas para actualizar el estado[29]. Más que depender del sistema a estudiar, la matriz R depende del sensor con el cual se mide la salida del sistema. Por ello, la matriz de covarianza R suele obtenerse tomando un conjunto de muestras reales, con el sistema en cierto estado detenido, con las cuales se calcula la varianza del ruido en las mediciones[30]. Como las mediciones de 27 distintas variables en general no están correlacionadas, esta matriz de covarianza resulta casi siempre diagonal. La determinación de la matriz de covarianza Q de ruido en el proceso resulta bastante más compleja, ya que no se tiene la habilidad de observar el proceso directamente. La matriz de covarianza Q representa la cantidad de incerteza del modelo determinístico. Esto permite que algunas veces un modelo relativamente simple (pobre) pueda producir resultados satisfactorios, si se “inyecta” la suficiente incerteza en el modelo vía Q[30]. Esto sin embargo, hace que la imprecisión de la estimación, entendiéndose como el rango en torno a la estimación del estado donde puede encontrarse el estado real con cierta probabilidad fija, sea mayor. Una matriz Q con valores grandes indica que el sistema puede cambiar rápidamente y que poca confianza debe ser puesta en la estimación del estado a priori. Por el contrario, una matriz Q pequeña indica que el habrá poca diferencia entre la estimación a priori y el estado real. Esta matriz de covarianza usualmente se escoge diagonal, eligiendo no tener u obviando la correlación entre las diferentes fuentes de ruido de las distintas variables de estado. Las matrices Q y R determinan fuertemente el comportamiento del filtro de Kalman. En general se determinan con la ayuda de métodos estadísticos, pero muchas veces por simplicidad se determinan utilizando técnicas de prueba y error hasta obtener resultados satisfactorios. 4.2.4. Inicialización del filtro Al ser el filtro de Kalman discreto un algoritmo iterativo, la actualización temporal requiere de los estados anteriores, y por ende es necesario inicializar los valores del estado del filtro para la iteración k=0. A partir de la Figura 4.2 se observa que son dos las variables que requieren inicializarse: x̂ y P . El inicializar el estado estimado es sencillo. Si se conoce una estimación para el estado inicial, simplemente se aplica. Si no se tiene información sobre el estado inicial, lo usual es aplicar el vector nulo como estimación inicial. La otra variable que es necesario inicializar es P , que es la matriz de covarianza del error en la estimación para la iteración k=0. La matriz de covarianza del error en la estimación es una medida de la precisión de la estimación, que se define según la siguiente ecuación[30]: [ Pk = E ( xk − xˆ k )( x k − xˆ k ) T ] (4.11) Los elementos de esta matriz corresponden a la covarianza entre distintas componentes del vector de error en la estimación. Es una matriz simétrica, que contiene en la diagonal las “autovarianzas” de los errores de las estimaciones de cada variable de estado. Usualmente se asume para su inicialización que los errores de las distintas variables no están correlacionados entre sí, lo cual determina que la matriz P inicial sea diagonal. Es común completar la matriz con las cotas superiores que se conozcan para las varianzas del error de cada estado. Esto se muestra en (4.12), donde σi es el error máximo esperado para la estimación inicial de la variable estado i-ésima xi. 28 σ 12 0 K 0 0 σ 22 O M Po = M O O 0 0 K 0 σ2 N (4.12) 4.2.5. Limitaciones del filtro de Kalman Como ya se mencionó, la principal limitación del filtro de Kalman es que sólo es aplicable para sistemas lineales, lo cual reduce drásticamente el espectro de aplicaciones realizables. Sin embargo, como se verá en la sección siguiente, es posible utilizar el filtro de Kalman para sistemas no lineales haciendo linealizaciones de los mismos. Otro inconveniente que presenta el filtro de Kalman es que asume que las propiedades de las fuentes de ruido son conocidas, lo cual no siempre ocurre. Las matrices de covarianza Q y R son las que contienen esta información, y determinan fuertemente el comportamiento del filtro. Una inadecuada elección de estos parámetros puede resultar en una convergencia lenta, o un rango de convergencia muy amplio, o incluso hacerlo divergir del estado real. 4.3. Filtro de Kalman Extendido El filtro de Kalman clásico fue desarrollado para sistemas lineales. Sin embargo muchos sistemas son no lineales, como el de los motores paso-a-paso. Un método efectivo para linealizar ecuaciones no-lineales en torno a un punto son las series de Taylor. El algoritmo que emplea series de Taylor para aplicar el filtro de Kalman, se denomina filtro de Kalman Extendido (EKF). Este observador es capaz de manejar casi cualquier sistema no-lineal, con el costo de tener que calcular la serie de Taylor en cada muestra temporal. No se ha demostrado que el filtro de Kalman Extendido sea óptimo, como sí lo es el filtro de Kalman original. Sin embargo, esto no quiere decir que sea una mala solución. Por el contrario, esta versión del filtro en muchas ocasiones logra un muy buen desempeño. Un sistema no-lineal que esté sujeto a mediciones lineales, como se puede asumir para el presente estudio, puede describirse con las ecuaciones (4.13) y (4.14), donde x es el estado, y f(x,u) es una función no-lineal conocida con variable de entrada u. x& = f ( x , u) (4.13) y = Cx (4.14) En algunos sistemas no-lineales es posible separar la función no lineal en dos partes, una dependiente del estado del sistema y la otra de la entrada, como se muestra en (4.15). El modelo del motor paso-a-paso permite realizar esta separación. f ( x , u) = g ( x ) + Bu 29 (4.15) Es necesario aplicar dos pasos para poder aplicar el filtro de Kalman al sistema en (4.15), linealización y discretización. 4.3.1. Linealización Las series de Taylor se calculan en torno a un punto de operación previamente determinado. Sea este punto x̂ . Si se denota por Dg al jabobiano de la función g, el aplicar serie de Taylor a la función no-lineal de (4.15) da la siguiente expresión: ( g ( x ) = g ( xˆ ) + Dg ( xˆ ) ⋅ ( x − xˆ ) + O ( x − xˆ ) 2 ) (4.16) Si se desprecian los términos de orden superior, el sistema descrito por (4.13) puede reescribirse como: x& = g ( xˆ ) + Dg ( xˆ ) ⋅ ( x − xˆ ) + Bu = Dg ( xˆ ) ⋅ ( x − xˆ ) + f ( xˆ , u) (4.17) y = Cx (4.18) Finalmente, es necesario realizar el siguiente cambio de variables para que el sistema resulte en la descripción lineal en variables de estados: u* = f ( xˆ , u) − Dg ( xˆ ) ⋅ xˆ (4.19) De esta manera, el sistema no-lineal original de las expresiones (4.13) y (4.14), es aproximado por el siguiente sistema lineal: Tabla 4.3 – Sistema lineal obtenido al aproximar el sistema no-lineal x& = Dg ( xˆ ) ⋅ x + u* (4.20) y = Cx (4.21) 4.3.2. Discretización Como deseamos utilizar la versión discreta del filtro de Kalman, es necesario discretizar la ecuación (4.20) del sistema en tiempo continuo. Para ello, se mostrará la discretización del sistema en variables de estado genérico con parámetros constantes, mostrado en (4.22). x& = Ax + Bu (4.22) y = Cx (4.23) 30 Sea el T tiempo de muestreo escogido para el sistema discreto. Se asume que la entrada al sistema pasa por un Retenedor de Orden Cero, es decir que la entrada es constante en cada intervalo de tiempo k. Teniendo esto en consideración, se puede demostrar[31] que el sistema continuo (4.22) corresponde exactamente al sistema discreto (4.24), con la transformación de parámetros mostrada en la Tabla 4.4. xk = Ad x k −1 + Bd uk −1 (4.24) yk = C d xk (4.25) Las expresiones (4.26) y (4.27) contienen exponenciales de matrices, funciones que representan series infinitas. Para la implementación del filtro en tiempo real del filtro, es necesario aproximarlas para evitar carga computacional excesiva. Tabla 4.4 – Parámetros del sistema discretizado Ad = e AT (4.26) T Bd = ∫e At dt ⋅ B (4.27) t =0 Cd = C (4.28) Para intervalos de tiempo T pequeños, se tiene la siguiente aproximación para la exponencial matricial: e AT 2 ( AT ) = I + AT + + K ≈ I + AT 2! (4.29) Con esta aproximación, se obtienen los parámetros aproximados del sistema discretizado presentados en la Tabla 4.5. Tabla 4.5 – Parámetros aproximados del sistema discretizado Ad ≈ I + AT (4.30) T2 Bd ≈ BT + AB ≈ BT 2 (4.31) Cd = C (4.32) Utilizando estas expresiones se transforma el modelo linealizado del sistema no-lineal en tiempo continuo de la Tabla 4.3, al sistema en tiempo discreto que se muestra en la Tabla 4.6. 31 Tabla 4.6 – Aproximación lineal discretizada de un sistema no-lineal xk = (I + Dg ( xˆ k −1 )T )x k −1 + (IT )u*k −1 (4.33) yk = Cx k (4.34) Este sistema sí tiene las características que permiten la aplicación del filtro de Kalman como observador. 4.3.3. Algoritmo El algoritmo del filtro de Kalman Extendido es simplemente el del filtro de Kalman original aplicado al sistema linealizado y discretizado. Para ello hay que calcular en cada iteración las matrices A y B, correspondientes al sistema linealizado en torno al estado estimado de la iteración anterior. Como B resulta una constante, puede ser precalculada fuera de la iteración. También es necesario adicionar el cálculo (4.36) en cada iteración de la entrada redefinida u*. Ambos cálculos, el de A y de u*, requieren la previa evaluación del jacobiano Dg en el último estado estimado. Tabla 4.7 – Ecuaciones de predicción del filtro de Kalman discreto Ak −1 = (I + Dg ( xˆ k −1 )T ) (4.35) u*k −1 = f ( xˆ k −1 , uk −1 ) − Dg ( xˆ k −1 ) ⋅ xˆ k −1 (4.36) xˆ ′k = Ak −1 xˆ k −1 + (IT )u*k −1 (4.37) Pk′ = Ak −1 Pk −1 AkT−1 + Qk −1T 2 (4.38) Tabla 4.8 – Ecuaciones de corrección del filtro de Kalman discreto S k = CPk′C T + Rk (4.39) K k = Pk′C T S k−1 (4.40) xˆ k = xˆ ′k + K k ( yk − Cxˆ ′k ) (4.41) Pk = Pk′ − K k S k K kT (4.42) 4.3.4. Estimación de parámetros desconocidos Cuando el modelo del sistema a observar tiene algún parámetro desconocido, es posible estimarlo utilizando una versión ampliada del filtro de Kalman Extendido. Esto es posible debido a que el Filtro de Kalman tradicional es un observador óptimo para cualquier sistema lineal, y permite introducir los parámetros desconocidos como variables de estado del modelo. Al hacer 32 esto el vector de estado aumenta su dimensión en una unidad, y consecuentemente las matrices también aumentan de tamaño. Esto tiene el inconveniente que aumenta considerablemente la carga computacional del filtro. Las ecuaciones sobre como realizar este procedimiento pueden consultarse en [29]. 4.3.5. Comentarios La complejidad en el algoritmo del filtro de Kalman Extendido es bastante mayor que la del filtro de Kalman tradicional, pues debe realizar una linealización del sistema en cada iteración. Por ello, utiliza una mayor cantidad de trabajo matricial, lo cual conlleva un aumento considerable en la carga computacional. La discretización del sistema utiliza que los parámetros del sistema linealizado son constantes. Esto es válido pese a que en cada iteración se use un sistema de parámetros distintos, ya que el resultado de cada linealización es un sistema de parámetros constantes, cuya aproximación es válida sólo para esa iteración. En cuanto a la eficiencia del filtro, no se puede demostrar que este filtro sea estadísticamente óptimo como el filtro de Kalman tradicional. Su estabilidad y rendimiento depende de las características del sistema no-lineal donde se aplique, y para el caso específico del sistema del motor paso-a-paso esto será analizado mediante simulaciones computacionales. 4.4. Implementación del EKF para el motor paso-a-paso Para realizar la implementación del filtro de Kalman Extendido para el sistema del motor paso-a-paso, lo primero es llevar las ecuaciones obtenidas para el motor, resumidas en la Tabla 3.2, a un sistema en formulación de variables de estado. Sea el estado definido por (4.43): θ ω x= i 1 i 2 (4.43) Esta definición de estado determina las siguientes ecuaciones en variables de estado: Tabla 4.9 – Formulación en variables de estado del modelo del motor paso-a-paso x& = f ( x , u) = g ( x ) + Bu 33 (4.44) ω 1 ((− K m ⋅ i1 ⋅ sin(5θ ) + K m ⋅ i2 ⋅ cos(5θ ) − K d ⋅ sin(20θ ) ) − B ⋅ ω − TRL ) J 1 g( x) = (− i1 (t ) ⋅ R + ω ⋅ K m ⋅ sin(5θ ) ) L 1 (− i2 (t ) ⋅ R − ω ⋅ K m ⋅ cos(5θ ) ) L 0 0 0 V1 (t ) 0 u ( t ) = B= V2 (t ) 1L 0 0 1 L 0 0 1 0 y = Cx C = 0 0 0 1 (4.45) (4.46) (4.47) El jacobiano Dg de la función g(x) es el siguiente: 0 1 0 Km D g 2 ,1 −B J − sin(5θ ) J Km Dg ( x ) = 5ω ⋅ K m cos(5θ ) sin(5θ ) −R L L L 5ω ⋅ K m K sin(5θ ) − m cos(5θ ) 0 L L Dg2 ,1 = 0 Km cos(5θ ) J 0 −R L − 5K m (i1 cos(5θ ) + i2 sin(5θ ) ) − 20 K d ⋅ cos(20θ ) J J (4.48) (4.49) En la ecuación (4.45) el término TRL se refiere la suma del torque de carga más el resistivo por fricción. Las ecuaciones de la Tabla 4.9, junto con las ecuaciones (4.48) y (4.49) y el algoritmo del EKF resumido en las Tablas 4.7 y 4.8, reúnen toda la información necesaria para la implementación del filtro de Kalman Extendido como observador de estados para el motor pasoa-paso de 2 fases, salvo la determinación de las matrices de covarianza Q y R. La matriz R depende fundamentalmente del sensor con que hagan las mediciones, y por ello para las simulaciones se asumen valores para la varianzas en las mediciones de cada sensor. Para la matriz Q en cambio, por simplicidad se puede hacer un ajuste manual basado en la información que sabemos del sistema. Por ejemplo, como se sabe que las corrientes de fase cambian mucho más rápido que la posición del rotor, debiera hacerse mayor la covarianza para la corriente que para la posición, según lo establecido en la sección 4.2.3 del presente capítulo. Los matrices de covarianza Q y R serán ajustadas según los resultados de las simulaciones, y se usarán los siguientes valores como referencia inicial: 34 15 0 0 0 15 0 Q = 0 0 1 0 0 0 0 0 0 1 0 .1 0 R = 0 0 .1 (4.50) 4.5. Comentarios En el capítulo previo se estableció que era necesario observar la posición del rotor en el motor paso-a-paso, para poder calcular la estimación del torque del motor. En el presente capítulo nos abocamos a ver la posibilidad de emplear un filtro de Kalman para realizar las estimaciones, y como el sistema a analizar era no-lineal, se tuvo que aplicar la versión extendida del filtro. Posteriormente se desarrollaron todas las expresiones para poder aplicar el filtro de Kalman Extendido, para finalmente analizar su implementación para el motor paso-a-paso. En el siguiente capítulo se simulará el algoritmo desarrollado, para analizar su estabilidad y convergencia, ajustando las matrices de covarianza para encontrar los valores de mejor rendimiento. 35 Capítulo 5 Simulaciones y Resultados En el presente capítulo se resumen los resultados de las simulaciones del motor paso-a-paso y del filtro de Kalman extendido. Todas ellas fueron realizadas en Matlab versión 7.1. 5.1. Simulación motor paso-a-paso Las ecuaciones del motor paso-a-paso de imán permanente de 2 fases, obtenidas en el Capítulo 3, fueron simuladas empleando parámetros deducidos a partir de la información recopilada sobre los motores utilizados en las disqueteras de 3.5’’. La Tabla 5.1 muestra los valores de dichos parámetros. Tabla 5.1 – Parámetros del motor simulado Parámetro P R L B J V Km Kd Descripción Número de pares de polos Resistencia de fase [ohms] Inductancia de fase [H] Coeficiente de roce [N*m*s/rad] Momento de Inercia [N*m*s^2/rad] Voltaje de fase [V] Constante de torque [N*m/A] Torque de detención [N*m] Valor 5 20 0.3 1e-4 5e-5 5 0.1 0.005 El modelo del motor paso-a-paso fue implementando en un procedimiento en código Matlab y en diagramas de bloques de Simulink. El código del modelo corresponde al anexo A.1. En la Figura 5.1 se muestra en miniatura el diagrama de bloques del modelo del motor de 2 fases, implementado en Simulink. El diagrama de bloques en tamaño completo se adjunta en el anexo B.1. 36 w*km*sin() 5 ángulo Pares de polos Sin Cos cos sin 1 0.05 Km [N*m/A] Voltaje A 1 s v elocidad A Nº de vueltas Km*cos() 1 Km*sin() -K1/J[N*m*s^2/rad] -> Aceleración Salida (posición) 1 s dIa/dt -C- -K- -KRa [ohms] Torque Carga [NM] velocidad -> angulo Torque Motor Ia*Km*sin() -> Corriente A -C3 1/L[H] Salida (corrientes) 1 s dIb/dt -> Corriente B Salida (velocidad) 2 Ib*Km*cos() A RPMs -K- -KRb [ohms] -KB [N*m*s/rad] -> friccion viscosa w*Km*cos() 2 Voltaje B 1 s aceleracion -> velocidad Figura 5.1 - Modelo del motor paso-a-paso de 2 fases en Simulink (anexo B.1) 5.1.1. Simulación en modo full-step Se simuló el motor teniendo como voltajes de entrada trenes de pulsos en el modo full-step, de frecuencia 0.25[Hz]. Además, se consideró una carga de 0.01[N·m] la cual súbitamente se incrementa a 0.015[N·m] entre los 4 y 10 segundos de simulación. La Figura 5.2 muestra la respuesta del sistema para los estados posición y velocidad. La Figura 5.3 muestra la respuesta para las corrientes de fase, junto con el gráfico del torque desarrollado por el motor, calculado usando los estados mediante la ecuación (3.28). El código que ejecuta la simulación corresponde al anexo A.2. Figura 5.2 – A la izquierda: Respuesta de la posición del rotor a una secuencia de pulsos A la derecha: Respuesta de la velocidad del rotor a una secuencia de pulsos 37 Figura 5.3 – A la izquierda: Respuesta de las corrientes de fase del motor a una secuencia de pulsos A la derecha: Respuesta del torque desarrollado, a un cambio en el torque de carga A partir de los gráficos es posible observar que el motor sufre de sobre-oscilaciones al efectuar cada paso, lo cual es una característica de este modo de operación. Además, soporta el incremento de carga sin perder sincronismo (pasos), pese a que el incremento a 0.015 [N·m] corresponde al 83% del torque máximo de contención (holding torque) observado para el modelo. 5.1.2. Simulación en modo microstepping El modo microstepping consiste en alimentar las fases del motor con ondas sinusoidales generadas mediante modulación por ancho de pulsos (PWM). Esta técnica permite detener el motor en un ángulo menor al ancho del paso fundamental, y además elimina buena parte del ruido de giro generando un movimiento más fluido[17]. Se simuló este modo de operación utilizando dos señales sinusoidales de la misma frecuencia utilizada para el tren de pulsos, desfasadas en 90º. Bajo las mismas condiciones de torque de carga empleada para las simulaciones anteriores, se obtuvo la respuesta mostrada en las Figuras 5.4 y 5.5. El código que ejecuta la simulación corresponde al anexo A.2. Figura 5.4 – A la izquierda: Respuesta de la posición del rotor a entradas sinusoidales (microstepping). A la derecha: Respuesta de la velocidad del rotor a entradas sinusoidales (microstepping) 38 Figura 5.5 – A la izquierda: Respuesta de las corrientes de fase del entradas sinusoidales (microstepping) A la derecha: Respuesta del torque desarrollado, a un cambio en el torque de carga en modo microstepping Al comparar la respuesta del modo microstepping con la del modo normal, que logra el mismo desplazamiento angular en el mismo tiempo, se observa que todas las variaciones en las distintas componentes del estado se producen más suavemente. En este caso no existe sobreoscilación en la posición, las presentes en la velocidad y en el torque son mucho más moderadas. 5.2. Simulación Filtro de Kalman Extendido con carga conocida El filtro de Kalman Extendido fue simulado de acuerdo con la implementación desarrollada en el Capítulo 4. Para esta simulación se consideró al torque de carga como entrada del sistema, para probar el desempeño del filtro de Kalman Extendido como estimador del estado completo, con énfasis en la estimación posición y de la velocidad del rotor. Las entradas del filtro son las entradas del sistema, es decir los voltajes y el torque de carga, más las observaciones simuladas de las corrientes de fase. El código que implementa esta simulación corresponde al anexo A.7. Mediante distintas pruebas de las simulaciones, se llegaron a los valores de las matrices de covarianza Q, R y Po mostrados en la Tabla 5.2. Los otros parámetros la simulación, que son la frecuencia de muestreo, el estado inicial, la estimación del estado inicial, y la desviación estándar del ruido blanco agregado a las mediciones de corriente, también se muestran en la tabla. Tabla 5.2 – Parámetros simulación filtro de Kalman Extendido Parámetro f σcorriente Q Descripción Frecuencia de muestreo [Hz] Desviación de ruido de medición corrientes [mA] Matriz de covarianza de ruido en el proceso 39 Valor 500 1.8 1000 0 Q= 0 0 0 0 0 1 0 0 0 1 0 0 0 1 R Matriz de covarianza de ruido en la medición xo Estado inicial 0 .1 0 R = 0 0 .1 T x o = (0 0 0 0 ) x̂ o Estimación del estado inicial T xˆ o = (0 0 0 0 ) Matriz de covarianza del error de la estimación inicial 0 0 0.01 0 0.01 0 0 0 Po = 0 0 0.625 0 0 0 0 0 . 625 Po Se simuló el filtro de Kalman simultáneamente con el modelo del motor paso-a-paso, este último con los mismos parámetros y entradas empleadas en la sección 5.11, incluyendo al incremento del torque. Los resultados de las estimaciones se muestran en los gráficos de la Figura 5.6, que incluyen el rango de encontrar el estado con un 95% de probabilidad (según la variable P del filtro de Kalman). Figura 5.6 – Arriba a la izquierda: Posiciones reales y estimadas. Arriba a la derecha: Error en la estimación de la posición. Abajo a la izquierda: Error en la estimación de la velocidad. Abajo a la derecha: Error en la estimación de la corriente en la fase A En la Figura 5.7 se muestra la estimación de la corriente de fase b, junto con las mediciones simuladas. 40 Figura 5.7 – Corriente de fase medida, estimada y real De los resultados se puede apreciar que el filtro logra estimar correctamente todos los estados del motor paso-a-paso. En particular la posición estimada tiene un error máximo del 20% del tamaño del paso fundamental del motor. 5.3. Simulación Filtro de Kalman Extendido con carga desconocida Tal como se planteó el problema original a resolver, la carga es una variable desconocida para el instrumento estimador que determina el torque desarrollado por el motor. En los sistemas reales, a veces es posible entregar un valor medio para ella. Sin embargo no es posible determinar sin tener un acceso al eje del motor, las variaciones que la carga pueda sufrir durante su operación, como sería por ejemplo el incremento de ella en alguna posición del rotor. Se hicieron dos simulaciones distintas para enfrentar el problema de la carga desconocida. En la primera se asume la carga como constante, y se analiza el desempeño del observador. En la segunda, se intenta estimar el torque de carga considerándolo como un parámetro desconocido que el filtro de Kalman debe estimar. 5.3.1. Simulación asumiendo una carga nominal constante Se simuló el mismo filtro empleado en la sección 5.2 pero ahora no utilizando el torque de carga como entrada para el filtro, sino considerándolo como una constante de valor cero. Para esta prueba se utilizó el modo microstepping del motor paso-a-paso, ya que produce señales más limpias. Para el modelo del motor, se utilizó un torque de carga constante de valor 0.01[N·m] el cual se incrementa un 50% en el intervalo entre los 10 y 11 segundos. El único parámetro distinto a los utilizados previamente para el filtro de Kalman, es la matriz de covarianza que se muestra en (5.1). La Figura 5.8 muestra el resultado de la simulación donde el torque desarrollado por el motor fue calculado mediante la ecuación (3.28). El código que implementa esta simulación corresponde al anexo A.7. 41 10 5 0 0 10 5 Q= 0 0 0 0 0 0 1 0 0 0 0 1 (5.1) Figura 5.8 – A la izquierda: Posiciones reales y estimadas A la derecha: Torque desarrollado real y estimado A partir de la Figura 5.8 se puede observar que la posición estimada sigue convergiendo, incluso cuando se considera a la carga como nula. De la Figura 5.8b se desprende que aunque el observador de esta forma es incapaz de estimar el valor medio del torque producido, si detecta los cambios abruptas transitorios. 5.3.2. Simulación utilizando la carga como parámetro a estimar Esta simulación ya no considera el torque de carga como una entrada del filtro de Kalman, sino como un parámetro desconocido a estimar. Los parámetros empleados para la simulación de este observador son los que se muestran en la Tabla 5.3. El código que implementa esta simulación corresponde al anexo A.8. Tabla 5.3 – Parámetros simulación filtro de Kalman Extendido para carga desconocida Parámetro f σcorriente Descripción Frecuencia de muestreo [Hz] Desviación de ruido de medición corrientes [mA] Q Matriz de covarianza de ruido en el proceso R Matriz de covarianza de ruido en la medición 42 Valor 250 1.8 10 −3 0 0 10 −3 Q= 0 0 0 0 0 0 0 0 1 0 0 0 0 0 1 0 0 .1 0 R = 0 0 .1 0 0 0 0 10 xo Estado inicial x o = (0 0 0 0 0 ) x̂ o Estimación del estado inicial T xˆ o = (0 0 0.25 0 0 ) Po Matriz de covarianza del error de la estimación inicial T 1 0 Po = 0 0 0 0 1 0 0 0 0 0 0 1 0 0 1 0 0 0 0 0 0 0 10 En las Figura 2.1 se muestran los resultados de las simulaciones, obtenidas utilizando el modo microstepping del motor paso-a-paso. Se observa que, pese a que la estimación del torque de carga realizada por el filtro de Kalman no converge, sí lo hacen el resto de las variables de estado; lo que es más importante, el torque calculado a partir de la ecuación (3.28) sí converge al torque real desarrollado por el motor. Figura 5.9 – Arriba a la izquierda: Posiciones reales y estimadas. Arriba a la derecha: Torque desarrollado real y estimado utilizando ecuación de torque. Abajo a la izquierda: Error en la estimación de la posición. Abajo a la derecha: Torque desarrollado real y carga estimada 43 5.4. Comentarios Como se ha podido observar en este capítulo, es posible estimar el torque desarrollado por motor paso-a-paso utilizando el filtro de Kalman Extendido. Aunque se empleó el filtro para estimar el torque de carga directamente, éste no convergió al valor esperado. Sin embargo, la inclusión de esta variable en el filtro permitió la correcta estimación del torque desarrollado, utilizando la ecuación de torque aplicada a las estimaciones de posición y corrientes obtenidas. 44 Capítulo 6 Conclusiones 6.1. Recapitulación del trabajo realizado Para iniciar el desarrollo del instrumento estimador de torque en tiempo real, primero se efectuó un estudio teórico sobre los motores paso-a-paso, el cual derivó a un modelo no-lineal del comportamiento electromecánico del mismo. Con este modelo se encontró que la relación corriente-torque, piedra angular para el desarrollo del instrumento deseado, era dependiente además de la posición angular del rotor. Como no se deseaba depender de mediciones de posición, se buscó un observador que fuera capaz de estimar ese estado del sistema, incluso bajo condiciones de ruido de las mediciones. La versión para sistemas no-lineales del Filtro de Kalman, llamada Filtro de Kalman Extendido, fue elegida como el observador adecuado. Tanto el modelo del motor, como el filtro, fueron implementados con funciones de bajo nivel, y simulados en Matlab, con el fin de analizar su comportamiento y permitir una posterior implementación del código en un PIC o DSP, para el desarrollo del instrumento. 6.2. Aspectos positivos y negativos del trabajo Pese a que no fue posible una estimación del torque a partir de sólo las mediciones de corriente, se pudo realizar la estimación incluyendo mediciones de voltajes, sin alterar el propósito del sistema. El utilizar el filtro de Kalman como observador resulta favorable, pues está diseñado para manejar sistemas con señales ruido aleatorio en el proceso y en la medición. Por otra parte, el hecho de tener que realizar linealizaciones constantemente en cada iteración, hace del Filtro de Kalman Extendido un algoritmo lento, con mucha carga de procesamiento. 6.3. Eventuales aplicaciones del trabajo desarrollado Como se comentó en la introducción, el disponer una realización física del instrumento permitiría analizar el comportamiento del torque desarrollado, con ello realizar un mantenimiento 45 predictivo del motor. Se podrían detectar fallas incipientes antes de que se extiendan y detengan el proceso productivo. Además, el conocer los puntos de operación de alto torque podría ayudar a determinar si en algún momento el motor se “salta” algún paso, perdiendo sincronización. Por otra parte, como la aplicación desarrollada utiliza un filtro de Kalman que produce estimaciones de todos los estados, podría adaptarse para proporcionar información online sobre la posición y velocidad del motor, sin necesidad usar sensores de posición. 6.4. Sugerencias para futuras investigaciones sobre el tema Se propone realizar la construcción de un prototipo del instrumento, utilizando transductores y un DSP, y realizarle pruebas de desempeño para compararlo con los resultados obtenidos en este trabajo; la construcción del prototipo debería involucrar un estudio del costo de la producción del instrumento. También se propone extender el trabajo a motores paso-a-paso de más fases. Otro proyecto interesante sería utilizar retroalimentación de los estados estimados del sistema para mejorar las características dinámicas del motor, con respecto a las que se tienen bajo el esquema de control por lazo abierto. 6.5. Comentarios finales Los motores paso-a-paso son actuadores de precisión, posicionadores de excelencia utilizados ampliamente en aplicaciones de control. Clásicamente se controlan mediante esquema de lazo abierto, pero su rendimiento puede ser mejorado sustancialmente usando estrategias de control de lazo cerrado. A su vez, el filtro de Kalman es una herramienta eficaz para estimar estados no accesibles de sistemas sometidos a contaminación por ruido que sigan distribuciones normales con valor medio nulo, tanto sean ruidos del proceso como de medición. En este contexto, en esta memoria se encontró que la versión extendida del filtro de Kalman se puede emplear en los motores paso-a-paso para estimar los estados posición y velocidad del rotor, a partir de mediciones de corrientes y voltajes de fases. Finalmente, creo que es importante que estas técnicas o tecnologías formen parte de los programas de asignaturas de la carrera de Ingeniería Civil Electricista de nuestra universidad, tales como “Control de Sistemas” u otros, puesto que son herramientas sumamente útiles, que todo ingeniero debiera conocer, y que a la fecha no están incluidas en dichos programas. 46 Referencias [1] PERSSON, J. y PERRIARD, Y. (2003): An optimized Extended Kalman filter algorithm for Hybrid Stepper Motors. En: IECON '03. The 29th Annual Conference of the IEEE. Vol. 1, pp. 297-300. [2] PERSSON, J. y PERRIARD, Y. (2003): Steady state Kalman filtering for sensorless control of hybrid stepper motors. En: IEMDC'03. IEEE International. Vol. 2, pp. 1174-1177. [3] KRISHNAMURTHY, P. y KHORRAMI, F. (2002): Permanent magnet stepper motor control via position-only feedback. En: Proceedings of the American Control Conference 2002. Vol. 4, pp. 3180-3185. [4] BODSON, M. y CHIASSON, J. (1989): Application of nonlinear control methods to the positioning of a permanent magnet stepper motor. En: Proceedings of the 28th IEEE Conference on Decision and Control. Vol.1, pp. 531-532. [5] ZRIBI, M. y CHIASSON, J. (1991): Position control of a PM stepper motor by exact linearization. En: IEEE Transactions on Automatic Control. Vol. 36, Issue 5, pp. 620-625. [6] BODSON, M. y CHIASSON, J. (1993): High-performance nonlinear feedback control of a permanent magnet stepper motor. En: IEEE Transactions on Control Systems Technology. Vol. 1, Issue 1, pp. 5-14. [7] MELKOTE, H. y KHORRAMI, F. (1997): A unified approach to adaptive nonlinear control of stepper motors. En: Proceedings of the 36th IEEE Conference on Decision and Control. Vol. 3, pp. 2495-2500. [8] BEHAL, A., “et al.” (2000): Sensorless rotor velocity tracking control of the permanent magnet stepper motor. En: Proceedings of the 2000 IEEE International Conference on Control Applications, pp. 150-155. [9] DE LEÓN-MORALES, “et al.”. (2005): Observer-based controller for position regulation of stepping motor. En: IEEE Proceedings - Control Theory and Applications. Vol. 152, pp. 465- 476. 47 [10] WALE, J. y POLLOCK, C. (1999): A Low-Cost Sensorless Technique for Load Torque Estimation in a Hybrid Stepping Motor. En: IEEE Transactions on Industrial Electronics. Vol. 46, pp. 833-841. [11] BLAUCH, A. y BODSON, M. (1993). High-speed parameter estimation of stepper motors. En: IEEE Transactions on Control Systems Technology. Vol. 1, pp. 270279. [12] DE CARVALHO, “et al.” (2003): A DSP based torque meter for induction motors. En: The 29th Annual Conference of the IEEE Industrial Electronics Society 2003. IECON '03. Vol. 1, pp. 414- 418. [13] KLIMAN, “et al.” (1996); A demonstration of a sensorless torque meter for AC motors. En: Industry Applications Conference, Thirty-First IAS Annual Meeting, IAS '96, Conference Record of the 1996 IEEE. Vol. 1, pp. 633-637. [14] SHAH, Chirayu (2004): Sensorless Control of Stepper Motor using Kalman Filter. Tésis (Master of Science in Electrical Engineering). Cleveland State University, Department of Electrical and Computer Engineering. [en línea] <http://academic.csuohio.edu/embedded/publications.html> [consulta: 20 diciembre 2006] [15] ERICSSON MICROELECTRONICS AB. (1998): Stepper Motor Basics. [Application Note]. Ericsson Microelectronics AB (Actualmente Infineon Technologies). [16] CONDIT, R. y JONES, D. (2004): Stepping Motor Fundamentals [Application Note]. Microchip Technology Inc., Literature Number: AN907. [17] JONES W., Douglas (1998): “Control of Stepping Motors, A Tutorial", The University of Iowa, Department of Computer Science, [en línea] <http://www.cs.uiowa.edu/~jones/step> [consulta: 20 diciembre 2006] [18] NMB TECHNOLOGIES CORPORATION (2003): NMB Motor Introduction: Stepper Motor Engineering [Tutorial]. NMB Technologies Corporation. [19] GUMMA, Saikiran. (2004): A Radial Basis Function Neuro Controller for Permanent Magnet Stepper Motor. Tésis (Master of Science in Electrical Engineering). Cleveland State University, Department of Electrical and Computer Engineering. [en línea] <http://academic.csuohio.edu/embedded/publications.html> [consulta: 19 enero 2007] [20] ZHU, Jian (2000): Electromechanical Systems [Lecture Notes]. University of Technology, Sydney (UTS). Centre for Electrical Machines and Power Electronics (CEMPE), Capítulo 10. [21] KENJO, Takashi (1984): Stepping motors and their microprocessor controls. 2da Edición. Oxford University Press. ISBN 019-8-59385-6 48 [22] ACARNLEY, Paul (1982): Stepping motors: a guide to modern theory and practice. Peter Peregrinus Ltd. ISBN 0-86341-027-8 [23] SLEMON, G. (1992): Electric machines and drives. Addison-Wesley. ISBN 020157-885-9 [24] PARKER Hannifiín Corporation (1996): Engineering Reference and Application Solutions: Motor Technologies. En: Compumotor Step Motor & Servo Motor Systems And Controls 1996/1997. Parker Hannifin Corporation, Compumotor Division. [25] ROMO L., Jorge (2002): Motores Eléctricos en Aplicaciones de Control, En: Apuntes de curso de perfeccionamiento para UnderFire. [26] SANZ F., Javier (2002): Máquinas Eléctricas. Cap.9. Prentice Hall. ISBN 842053-391-2 [27] KUPHALDT, T. (2006): Lessons In Electric Circuits. Vol. II – AC. [en línea] <http://www.ibiblio.org/obp/electricCircuits/AC/index.html> [consulta: 7 enero 2007] [28] JACKSON, J. D. (1999): Classical Electrodynamics. New York. John Wiley & Sons. 3era Edición. ISBN 047-1-30932-X [29] PERSSON, Jan (2005): Innovative standstill position detection combined with sensorless control of synchronous motors. Tésis (Grade de Docteur Ès Sciences). École Polytechnique Fédérale de Lausanne (EPFL), Institut de production et robotique (IPR), Suiza. [en línea] <http://library.epfl.ch/en/theses> [consulta: 6 mayo 2007] [30] WELCH, G. y BISHOP, G. (2006): An Introduction to the Kalman Filter. University of North Carolina at Chapel Hill, Department of Computer Science. [en línea] <http://www.cs.unc.edu/~welch/kalman/kalmanIntro.html> [consulta:1 mayo 2007] [31] WIKIPEDIA, THE FREE ENCYCLOPEDIA (200-): Discretization [en línea] <http://en.wikipedia.org/wiki/Discretization> [consulta: 9 febrero 2007] 49 Anexo A - Códigos Matlab A.1 stepper_rbl.m Es una función que a partir del estado x, y los parámetros del motor, genera el estado x´. Llama a la función genera_voltajes.m para generar la entrada para el motor, elegible entre los modos full-step y microstepping. % Esta funcion describe la dinamica del motor paso a paso function xdot=stepper_rbl(t,x) % funcion stepper_rbl global Km Kd R L N B_roce J Vnom f modo_stepper% declare global variables %Definicion de estado: %x=[theta; w ; ia ; ib] %x=[ x(1); x(2); x(3); x(4)] % Cálculo de los voltajes de entrada: voltajes=genera_voltajes(t); Va=voltajes(1); Vb=voltajes(2); %Establecer xdot como un vector columna: xdot=zeros(4,1); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %CÁLCULO DE LA DERIVADA DEL ESTADO % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %theta punto es omega xdot(1)=x(2); % omega punto es alfa: (acá se puede incluir o no el término con Kd) xdot(2)=(-Km*x(3)*sin(x(1)*N/2) +Km*x(4)*cos(x(1)*N/2) -Kd*sin(x(1)*4*N/2) -B_roce*x(2) -(torque_carga(t)))/J; %ia punto: xdot(3)=(Va-R*x(3)+Km*x(2)*sin((N/2)*x(1)))/L; %ib punto: xdot(4)=(Vb-R*x(4)-Km*x(2)*cos((N/2)*x(1)))/L; A.2 run_stepper.m Es una función que simula un motor paso-a-paso, utilizando para ello el código stepper_rbl.m. Define los parámetros del motor, y utiliza el función de Matlab ode23 para resolver la ecuación diferencial. Posteriormente grafica los estados del motor: la posición, la velocidad y las corrientes de fase. También calcula y grafica el torque desarrollado. % Simulacion de un Stepper motor PM en Matlab clear; close all; %-------------------------SISTEMA REAL------------------------------------% %Parámetros del Motor paso-a-paso: %declarar las variables globales (los parametros constantes del motor) 50 global Km Kd R L N B_roce J Vnom f TcargaRef tf modo_stepper disp('Simulación motor paso-a-paso:'); disp(' '); echo on; %Define parámetros motor(en MKS) Km=0.1; %N*m/A (Constante de torque) Kd=Km/20; R=20; %Ohms L=0.03; %Henrys N=10; %Polos del rotor (N=2*P). P es el número de pares de polos B_roce=1e-4; %N*m*s/rad J=5e-5; %N*m*s^2/rad Vnom=5; %Volts f=1/4; %Hz (Frecuencia pulsos: es el periodo del tren de pulsos, cada pulso individual es de f'=1 (o f'=2 en halfstep)) %obs: f=1/4*20; es la frec máxima para TcargaRef=0.01; TcargaRef=0.01; %N*m (Torque de carga de referencia, ver torque_carga.m) modo_stepper=0; %Modo del motor para entrada de trenes de pulso (1 o 1/2), o para entrada sinusoidal "microstepping" (0 o -1) %modo 2 para V=cte, y 3 para V=0 (para ver holding torque y detent) %Definicion del estado: x=[theta, w, ia, ib] %Estado inicial: xo=[0.0*(2*pi), 0, 0, 0]; %Parámetros de la simulación: to=0; tf=(1/f)*N/2; %Tiempos inicial y final (antes tf=20 para f=1/4) muestras=10000; %Nº de muestras a tomar delta_t=tf/muestras; %Intervalo del tiempo discreto (delta_t=0.002 <=> fs=500Hz frecuencia de muestreo) echo; tspan=[0:delta_t:tf]; %tspan=[to tf]; % Lapso temporal (time span) % Lapso temporal (time span) disp(['Pasos de ' num2str(180/N) 'º = ' num2str(1/(2*N)) ' vueltas'] ); %Cálculo del estado (integracion): % La rutina de integracion 'ode23' llama a % para resolver las dinámicas del sistema [t,x]=ode23('stepper_rbl', tspan, xo); la funcion 'stepper-rbl' %El torque real: Tm=Km*(-x(:,3).*sin((N/2)*x(:,1)) + x(:,4).*cos((N/2)*x(:,1))) Kd*sin((4*N/2)*x(:,1)); %El torque de carga: Tcarga=[]; for contador=1:length(t) temporal=torque_carga(t(contador)); Tcarga=[Tcarga; temporal]; end %{ % Grafica las variables de estado figure(1); 51 plot(t,x(:,1)/(2*pi)); xlabel('Tiempo (s)'); ylabel('Estado posicion Theta (en Vueltas)'); title('Respuesta del Motor paso-a-paso'); grid on; figure(2); plot(t,x(:,2)*(60/(2*pi))); xlabel('Tiempo (s)'); ylabel('Estado velocidad w (RPMs)'); title('Respuesta del Motor paso-a-paso'); grid on; figure(3); plot(t,x(:,3),'k',t,x(:,4),'k:'); xlabel('Tiempo (s)'); ylabel('Estado ia e ib(A)'); title('Respuesta del Motor paso-a-paso'); legend('Corriente Ia','Corriente Ib',0); grid off; %figure; %plot(t,x(:,4)) %xlabel('Tiempo (s)') %ylabel('Estado ib (A)') %title('Respuesta del Motor paso-a-paso') %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %Grafica la estimación del torque desarrollado%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure(4); plot(t,Tm,'b--'); hold on; plot(t,Tcarga,'g'); grid off; title('Torque desarrollado v/s tiempo'); legend('Torque motor','Torque carga',0); xlabel(['Tiempo (s)']); ylabel('Torque Tm [N*m]'); hold off; disp('Analisis de Posicion:'); disp(['Posicion Mínima (en fraccion de vueltas): ' num2str(min(x(:,1)*(1/(2*pi))))]); disp(['Posicion Máximo (en fraccion de vueltas): ' num2str(max(x(:,1)*(1/(2*pi))))]); disp(' '); disp('Analisis de Velocidad:'); disp(['Velocidad esperada dada la frecuencia: ' num2str(60*4*f/(2*N)) ' RPMs']); disp(['Velocidad Mínima: ' num2str(min(x(:,2)*(60/(2*pi)))) ' RPMs']); disp(['Velocidad Máximo: ' num2str(max(x(:,2)*(60/(2*pi)))) ' RPMs']); disp(['Velocidad Media : ' num2str(mean (x(:,2))*(60/(2*pi))) ' RPMs']); disp(' '); disp('Analisis de Torque:'); disp(['Torque Mínimo: ' num2str(min(Tm)) ' N*m']); disp(['Torque Máximo: ' num2str(max(Tm)) ' N*m']); disp(['Torque Medio : ' num2str(mean (Tm)) ' N*m']); disp(' '); %} 52 disp('Fin de simulación del motor paso-a-paso.');disp(' '); %-------------------FIN SISTEMA REAL--------------------------------------% A.3 torque_carga.m Esta función define el torque de carga en el tiempo, para usarla con stepper_rbl.m. % Esta funcion define el Torque de carga como funcion del tiempo function tcarga=torque_carga(t) % funcion stepper_rbl global f TcargaRef tf% declare global variables %Tl es el modulo o amplitud del torque de carga %Nulo %tcarga=0; %Constante %tcarga=t-t+ TcargaRef; %tcarga=TcargaRef; %Sinusoidal con frecuencia de un cuarto de la velocidad del motor %tcarga=TcargaRef*0.1+TcargaRef*0.9*sin(2*pi*f*t/4); %Torque In-crecendo %tcarga=6*TcargaRef*t/(2/f); %%{ %Carga + Torque Brusco (*2) tcarga=TcargaRef; if(10<t && t<11) %(4<t && t<10) tcarga=1.5*TcargaRef; %aguanta hasta 1.7 end %} %{ %Torque Brusco tcarga=0; if(4<t && t<10) tcarga=TcargaRef; end %} tcarga=20*Tl*t/(2/f); A.4 genera_voltajes.m Esta función genera las señales de voltajes de entrada para el motor. Según el modo de operación, genera las señales del tipo Full-Step, Half-Step y Microstepping. Se utiliza en run_stepper_rbl.m. %GENERA VOLTAJES TIPO TREN DE PULSOS PARA EL MOTOR PASO-A-PASO 53 function V=genera_voltajes(t) global Vnom f modo_stepper %el argumento f es la frecuencia del tren de pulsos a generar V=[]; %disp('Generando Voltajes de Entrada:'); %if(modo_stepper==1) % disp('MODO ONE-STEP (1 bobina a la vez)'); %else % disp('MODO HALF-STEP'); %end %Angulo en radianes entre 0 y 2*pi:(w_e*t=2*pi*f*t resto de 2*pi) angulo=rem(2*pi*f*t,2*pi); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %Definición de las entradas:%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%Entrada constante%%%% %(con modo_stepper=2): %%%%Trenes de pulsos%%%%% %MODO ONE-STEP (con modo_stepper=1): %Aca la "vuelta" se divide en 4 zonas, y se usa el siguiente esquema: %(10,01,-10,0-1,...) %MODO HALF-STEP (con modo_stepper=1/2): %Aca la "vuelta" se divide en 8 zonas, y se usa el siguiente esquema: %(10,11,01,-11,-10,-1-1,0-1,1-1,...) %%%%Entrada sinusoidal%%% %(con modo_stepper=0): %%%%Entrada sinusoidal cortada%%% %(con modo_stepper=-1): %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Va=0; Vb=0; if(modo_stepper==1) if ((abs(angulo)<= (pi/4)) ||(abs(angulo-2*pi)<= (pi/4)) ) Va=Vnom; end if (abs(angulo-(pi/2)) <= (pi/4)) Vb=Vnom; end if (abs(angulo-pi) <= (pi/4)) Va=-Vnom; end if (abs(angulo-(3*pi/2)) <= (pi/4)) Vb=-Vnom; end 54 end if(modo_stepper==1/2) if ((abs(angulo)-pi/8)<= (3*pi/8)) ||(abs(angulo-2*pi-pi/8)<= (3*pi/8)) Va=Vnom; end if (abs(angulo-(pi/2)-pi/8) <= (3*pi/8)) Vb=Vnom; end if (abs(angulo-pi-pi/8) <= (3*pi/8)) Va=-Vnom; end if (abs(angulo-(3*pi/2)-pi/8) <= (3*pi/8)) Vb=-Vnom; end end if(modo_stepper==0) Va=Vnom*cos(angulo); Vb=Vnom*sin(angulo); end if(modo_stepper==(-1)) if(3<t && t<5) t=3; end if(t>=5) t=t-2; end angulo=rem(2*pi*f*t,2*pi); Va=Vnom*cos(angulo); Vb=Vnom*sin(angulo); end if(modo_stepper==2) Va=Vnom; Vb=0; end if(modo_stepper==3) Va=0; Vb=0; end V=[Va, Vb]; 55 A.5 kalman_iteracion_v2.m Implementa un filtro de Kalman lineal discreto. Recibe los parámetros del sistema lineal, más el vector estimación del estado xest(k-1), y la matriz de covarianza del error en la estimación P(k1), y retorna los valores actualizados de xest(k) y P(k). function salidakalman = kalman_iteracion_v2(xest,P,y,u,A,B,C,Q,R); %%%%%%%%% Filtro de Kalman%%%%%%%%%%%%%%%%%%%% %(Versión Persson) %%%% Predicción %%%% xest=A*xest+B*u; %calculo la estimacón a priori, %P=A*P*A'+Q; %usa xest y Pdel k anterior P=A*P*A'+B*Q*B'; %<--- Version PERSSON (no equivalente) %%%%%%%%%%%%%%%%%%%% %%%% Corrección de Medición %%% S=C*P*C'+R; K=P*C'*inv(S); xest=xest+K*(y-C*xest); %P=P-K*C*P; %<--- Version Original P=P-K*S*K'; %<--- Version PERSSON (son equivalentes) %Hacer P simetrica%% P=(P+P')/2; %%%%%%%%%%%%%%%%%%%% salidakalman=[xest, P]; A.6 ejecucion_kalman_codigo.m Genera un sistema lineal discreto, contaminado por una señal de ruido gaussiana en la entrada y en la salida, para ejecutar y probar un filtro de Kalman usando la función kalman_iteracion_v2.m. Simula el sistema el tiempo determinado el parámetro tiempo_simulación (en segundos), y luego de cada tiempo de muestreo T calcula los nuevos estados, y estados estimados por el filtro de Kalman. Posteriormente grafica los estados reales y estimados, el error en las estimaciones, y también el rango donde debería encontrarse el estado real, en torno al estimado, con una probabilidad del 95%. clear all; %Prueba del Filtro de Kalman Discreto (Versión 2) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %Sistema Distreto Simple (2x2): % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %x(k+1)=A*x(k)+B*u(k) % %y(k)=C*x(k) % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %Parametros simulación: tiempo_simulacion=30 %tiempo de simulacion [s] T=0.01 %T=Tiempo de muestreo[s] muestras=tiempo_simulacion/T x0=[10;-10] %Estado real inicial ([m];[m/s]) 56 P0=diag([12*12;15*15]) %Covarianza del error del estado inicial ([m];[m/s]) sigmaplanta=10 %desviacion de la aceleración (ruido) (m/(s)^2) (para covarianza de ruido del proceso) sigmamedicion=10 %desviacion estandar de las mediciones (m) (para covarianza de ruido de medición) %----------------------------------SISTEMA--------------------------------%%%%%%%%%%%%%%%%%%%%%%%%%%%% %EJEMPLO: Partícula en 1D % %sometida a acel. cte % %con Masa unitaria. % %%%%%%%%%%%%%%%%%%%%%%%%%%%% %Partícula acelerando a 1[m/(s^2)] %Partiendo en el origen x=0, con Vx=10[m/s] %posición: x(k+1)=x(k)+dt*v(k)+(dt^2)/2*a(k) %velocidad: v(k+1)=v(k) +dt*a(k) %la entrada es la aceleración a(k) %el ruido del sistema viene de aceleraciones aleatorias %mido solamente la posición (con ruido) %estimare la posición real, y la velocidad %Variables de estado (sistema real): x=[]; %x(k)=[posicion;velocidad] %xtest=[]; %%%%%%[prueba1] vector de estado sistema real aprox al discretizarlo (T^2=0) %Entrada del sistema u=[10;0] %Fuerza=Aceleración*10 (cte) %Estado Inicial: %x0=[10;-10] %Parámetros sistema: A=[1 T;0 1] B=[T^2, 0; T, 0] Btest=[0, 0; T, 0]; %%%[prueba1] parametro del sistema real con aprox en la discretización (T^2=0) C=[1,0] %[1,0]:la posición es observable, no la velocidad (asi el sistema es observabe) <--seguro? %Inicialización variables sistema: estado=x0; %Condicion Inicial x(:,1)=x0; %------------------------------SISTEMA RUIDOSO----------------------------%sigmaplanta=0.1; %sigmamedicion=0.1; %------------------------------FILTRO DE KALMAN---------------------------%Parámetros del Filtro %Q=eye(2) %matriz de covarianza del ruido del sistema (=R1) Q=diag([sigmaplanta^2 sigmaplanta^2]) %R=eye(1) %matriz de covarianza del ruido de la medición (=R2) R=diag([sigmamedicion^2]) G=B(:,1); %Transforma el ruido de acel a ruido de sistema. %Variables del filtro xest=[]; %estimación 57 S=[]; % covarianza de innovación P=[]; % covarianza del sistema %Estimación Inicial xest=[0;0] %Inicialización de variables filtro %P0=B*Q*B' % Error de covarianza inicial (Jan PERSSON metodo 1) %P0=[12*12 0;0 15*15]% Error de covarianza inicial (Jan PERSSON metodo 2) P=P0; %---------------------------------EXTRAS----------------------------------xestmatriz=[]; xestmatriz(:,1)=xest; Pmatriz=[]; Pmatriz(:,1)=diag(P); salidareal=[]; %Para graficar las mediciones salidareal(1)=(C*x0)+randn*sigmamedicion; %------------------------------ITERACIONES--------------------------------for k=2:muestras %%%%%%%%% El sistema real%%%%%%%%%%%%%%%%%%%%% estado=A*estado+B*u + G*randn*sigmaplanta; %x(k)=A*x(k-1)+B*u(k-1)+w(k-1) y=C*estado+randn*sigmamedicion; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%% Filtro de Kalman%%%%%%%%%%%%%%%%%%%% temporal=kalman_iteracion_v2(xest,P,y,u,A,B,C,Q,R); %porque salida de kalman_iteracion_v2 es [xest, P]; xest=temporal(:,1); P=[temporal(:,2) temporal(:,3)]; %%%%%%%%extras al sistema%%%% (para grabar valores) x(:,k)=estado; salidareal(k)=y; xestmatriz(:,k)=xest; %para guardar valores de estimaciones %Pmatriz(:,k)=diag(P); %graba la diagonal %Pmatriz(:,k)=eig(P); %graba la "diagonal" %[eig(P),diag(P),eig(P)-diag(P) ] $Comparación "Diagonales" Pmatriz(:,k)=max(eig(P),diag(P)); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %Grafica la posición real y la estimada%%%%%%%%%%%%%%%%%% figure(5); plot(x(1,:),'b'); hold on; plot(salidareal,'r+'); plot(xestmatriz(1,:),'g'); 58 plot(xestmatriz(1,:)-2*sqrt(Pmatriz(1,:)),'-y'); debiera estar el estado real plot(xestmatriz(1,:)+2*sqrt(Pmatriz(1,:)),'-y'); debiera estar el estado real grid on; %Grafica rango donde %Grafica rango donde legend('Posición real', 'Posición Medida','Posición estimada (Kalman)', 'Rango ubicación posición real (Kalman)',0); title('Posición v/s iteración'); xlabel(['Iteraciones (Cada una de ' num2str(T) '[s])']); ylabel('Posición [m]'); hold off; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %Grafica el error en la posición%%%%%%%%%%%%%%%%%%%%%%%%% figure(6); plot(x(1,:)-xestmatriz(1,:)); hold on; plot(-2*sqrt(Pmatriz(1,:)),'-y'); %Grafica rango donde debiera estar el estado real con 95% de probabilidad plot(+2*sqrt(Pmatriz(1,:)),'-y'); %Grafica rango donde debiera estar el estado real con 95% de probabilidad hold off; grid on; title('Error posición v/s iteración'); legend('Error posición', 'Rango ubicación posición real (Kalman)',0); xlabel(['Iteraciones (Cada una de ' num2str(T) '[s])']); ylabel('Error [m]'); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %Grafica la velocidad real y la estimada%%%%%%%%%%%%%%%% figure(7); plot(x(2,:),'b'); hold on; plot(xestmatriz(2,:),'g'); plot(xestmatriz(2,:)-2*sqrt(Pmatriz(2,:)),'-y'); %Grafica rango donde debiera estar el estado real con 95% de probabilidad plot(xestmatriz(2,:)+2*sqrt(Pmatriz(2,:)),'-y'); %Grafica rango donde debiera estar el estado real con 95% de probabilidad grid on; legend('Velocidad real','Velocidad estimada (Kalman,', 'Rango ubicación velocidad real (Kalman)',0); hold off; title('Velocidad v/s iteración'); xlabel(['Iteraciones (Cada una de ' num2str(T) '[s])']); ylabel('Velocidad [m/s)]'); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %Grafica el error en la velocidad%%%%%%%%%%%%%%%%%%%%%%%% figure(8); plot(x(2,:)-xestmatriz(2,:)); hold on; plot(-2*sqrt(Pmatriz(2,:)),'-y'); %Grafica rango donde debiera estar el estado real con 95% de probabilidad plot(+2*sqrt(Pmatriz(2,:)),'-y'); %Grafica rango donde debiera estar el estado real con 95% de probabilidad grid on;hold off; title('Error velocidad v/s iteración') legend('Error velocidad', 'Rango ubicación velocidad real (Kalman)',0); xlabel(['Iteraciones (Cada una de ' num2str(T) '[s])']); ylabel('Error [m/s)]'); 59 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% A.7 ejecucion_EKF_codigo.m Este código implementa el filtro de Kalman Extendido, considerando al torque de carga como una entrada al modelo. Utiliza la función kalman_iteracion_v2.m. %Versión del Filtro de Kalman Extendido Discreto (Versión 2)para el motor %stepper disp('Inicio algoritmo Filtro de Kalman Extendido'); disp(' '); %Antes, debe haber ejecutado el Modelo del motor paso-a-paso %que genera un vector columna 't' con los tiempos de muestra %y una matrix 'x' con los estados puestos en fila, para cada t: %x=[x1;x2;x3;x4]=[theta;omega;i1;i2] %(cada fila es un tiempo distinto) %tambien debe existir la variable delta_t %----------------------------------SISTEMA--------------------------------%El estado fue calculado previamente y existe en el workplace %Necesito los parámetros del sistema estimado, pero son calculados en cada %iteración salvo por B y C B=[0 0; 0 0; 1/L 0; 0 1/L] C=[0 0 1 0; 0 0 0 1] %-------------------------------------------------------------------------%Parametros simulación: %tiempo_simulacion=t(length(t)) %delta_t muestras=length(t); %tiempo de simulacion [s] %Tiempo de muestreo[s] %---------------------------FILTRO DE KALMAN------------------------------%-----------------Parámetros del Filtro-----------------------------------P0=diag([0.01, 0.01, 0.25*0.25, 0.25*0.25]) %Covarianza del error del estado inicial ([m];[m/s]) %sigmaplanta=10 %desviacion del modelo (ruido) (en distintas unidades) para covarianza de ruido del proceso Q %Q=eye(4) %matriz de covarianza del ruido del sistema %Q=diag([sigmaplanta^2, sigmaplanta^2, sigmaplanta^2, sigmaplanta^2]) %Q=diag([1, 10, 1, 1]) (feo se sale) %Q=diag([1, 1, 1, 1]) %Q=diag([10, 1, 1, 1]) Q=diag([1e5, 1e5, 1, 1]) %Q=diag([1e3, 1, 1, 1]) sigmamedicion=Vnom/R*0.007 mediciones(A) %desviacion estandar (ruido) de las %se relaciona con la covarianza de ruido de medición Rcov %sigmamedicion=sqrt(0.1) %0.007 es el ejemplo de precicion del transductor de corriente LEM LTS 15-NP %Rcov=eye(2) %matriz de covarianza del ruido de la medición %Rcov=diag([sigmamedicion^2 sigmamedicion^2]) 60 Rcov=diag([0.1 0.1]) %-------------------------------------------------------------------------%Variables del filtro xest=[]; %estimación S=[]; % covarianza de innovación P=[]; % covarianza del sistema %Definicion del estado: x=[theta, w, ia, ib] %Estimación Inicial xest=[0,0,0,0] %Inicialización de variables filtro %P0=B*Q*B' % Error de covarianza inicial (Jan PERSSON metodo 1) %P0=[12*12 0;0 15*15]% Error de covarianza inicial (Jan PERSSON metodo 2) P=P0; %---------------------------------EXTRAS----------------------------------xestmatriz=[]; xestmatriz(1,:)=xest; P_diagonal=[]; %para grabar los valores en la diagonal de P (autovarianzas) P_diagonal(1,:)=diag(P); salidareal=[]; %Para graficar las mediciones salidareal=[salidareal ; ( (C*xo')+[randn*sigmamedicion; randn*sigmamedicion] )' ]; %------------------------------ITERACIONES--------------------------------for k=2:muestras %%%%%%%%% El sistema real%%%%%%%%%%%%%%%%%%%%% i1=x(k,3); i2=x(k,4); y=[i1;i2]+[randn*sigmamedicion; randn*sigmamedicion]; %Medición del sistema, agrego ruido salidareal=[salidareal ;y']; %Guardo la medición u=(genera_voltajes(t(k-1))) ; es la entrada para el estado anterior en (k-1) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%% El sistema estimado%%%%%%%%%%%%%%%%% pos_est=xest(1); %esta es la estimación de la iteración anterior w_est=xest(2); ia_est=xest(3); ib_est=xest(4); Va=u(1); Vb=u(2); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%% Linealización y discretización%%%%%%%% %%Cálculo del jacobiano Dg%%%% Dg=[]; Dg=[0 1 0 0]; Dg21= -(N/2)*(Km/J)*( ia_est*cos((N/2)*pos_est) + ib_est*sin((N/2)*pos_est) ) - 4*(N/2)*Kd*cos(4*(N/2)*pos_est)/J; 61 %U Dg=[Dg ; Dg21, -B_roce/J, -(Km/J)*sin((N/2)*pos_est), (Km/J)*cos((N/2)*pos_est) ]; Dg=[Dg ; ((N/2)*w_est*Km/L)*cos((N/2)*pos_est), (Km/L)*sin((N/2)*pos_est), -R/L, 0 ]; Dg=[Dg ; ((N/2)*w_est*Km/L)*sin((N/2)*pos_est), (Km/L)*cos((N/2)*pos_est), 0 ,-R/L]; %%%%%%%%%%%%%%%% %%%%Cálculo función f%%%%% func_ es xpunto' func_f=[]; %theta punto es omega func_f(1)=xest(2); % omega punto es alfa: (acá se puede incluir o no el término con Kd) %func_f(2)=(-Km*xest(3)*sin(xest(1)*N/2) + Km*xest(4)*cos(xest(1)*N/2) B_roce*xest(2) -Kd*sin(xest(1)*4*N/2) -(torque_carga(t(k-1))))/J; % %Esto seria hacer trampa pues no conozco la carga exactamente func_f(2)=(-Km*xest(3)*sin(xest(1)*N/2)+Km*xest(4)*cos(xest(1)*N/2)B_roce*xest(2) -Kd*sin(xest(1)*4*N/2) -(TcargaRef*0))/J; %ia punto: func_f(3)=(Va-R*xest(3)+Km*xest(2)*sin((N/2)*xest(1)))/L; %ib punto: func_f(4)=(Vb-R*xest(4)-Km*xest(2)*cos((N/2)*xest(1)))/L; %%%%%%%%%%%%%%%% %%%u_asterisco%% u_asterisco=(func_f)' - Dg*(xest') ; %vector (columna) de entrada u* %%%%%%%%%%%%%%%% %%%La matriz A del sistema linealizado%%%%%%%% A=(eye(4)+Dg*delta_t); matriz A del sistema linealizado y discretizado %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%% Filtro de Kalman%%%%%%%%%%%%%%%%%%%% (llama al algoritmo clásico) temporal=kalman_iteracion_v2(xest',P,y,u_asterisco,A, (eye(4)*delta_t) ,C,Q,Rcov); xest=(temporal(:,1))'; %porque salida de kalman_iteracion_v2 es [xest, P]; P=[temporal(:,2) temporal(:,3) temporal(:,4) temporal(:,5)]; %porque salida de kalman_iteracion_v2 es [xest, P]; %%%%%%%%extras al sistema%%%%%%%%%%%%%%%%%%%%% (para grabar valores) xestmatriz(k,:)=xest; %guarda los valores de estimaciones para graficar xest P_diagonal(k,:)=diag(P); %guarda la diagonal para graficar P if(k==(round(muestras*0.25))) 62 %La disp('Kalman:25%') end if(k==(round(muestras*0.5))) disp('Kalman:50%') end if(k==(round(muestras*0.75))) disp('Kalman:75%') end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% end disp('Fin algoritmo Filtro de Kalman Extendido'); disp(' '); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %Estimación del torque desarrollado por el motor%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Tm_est=Km*(-xestmatriz(:,3).*sin((N/2)*xestmatriz(:,1)) + xestmatriz(:,4).*cos((N/2)*xestmatriz(:,1))) Kd*sin((4*N/2)*xestmatriz(:,1)); %El torque real (ya calculado en run_stepper): %Tm=Km*(-x(:,3).*sin((N/2)*x(:,1)) + x(:,4).*cos((N/2)*x(:,1))) Kd*sin((4*N/2)*x(:,1)); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %Grafica la posición real y la estimada%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure(5); plot(t,x(:,1)/(2*pi),'k'); hold on; %plot(salidareal,'r+'); plot(t,xestmatriz(:,1)/(2*pi),':k'); plot(t,xestmatriz(:,1)/(2*pi)-2*sqrt(P_diagonal(:,1))/(2*pi),'--k'); %Grafica rango donde debiera estar el estado real plot(t,xestmatriz(:,1)/(2*pi)+2*sqrt(P_diagonal(:,1))/(2*pi),'--k'); %Grafica rango donde debiera estar el estado real grid off; legend('Posición real', 'Posición estimada (Kalman)', 'Rango ubicación posición real (Kalman)',0); %legend('Posición real', 'Posición Medida','Posición estimada (Kalman)', 'Rango ubicación posición real (Kalman)',0); title('Posición v/s tiempo'); xlabel(['Tiempo (s)']); ylabel('Posición Theta (en Vueltas)'); hold off; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %Grafica el error en la posición%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure(6); plot(t,x(:,1)/(2*pi)-xestmatriz(:,1)/(2*pi),'r'); hold on; plot(t,-2*sqrt(P_diagonal(:,1))/(2*pi),'-y'); %Grafica rango donde debiera estar el estado real con 95% de probabilidad 63 plot(t,+2*sqrt(P_diagonal(:,1))/(2*pi),'-y'); donde debiera estar el estado real con 95% de probabilidad hold off; grid off; %Grafica rango title('Error posición v/s iteración'); legend('Error posición', 'Rango ubicación posición real (Kalman)',0); xlabel(['Tiempo (s)']); ylabel('Error (en Vueltas)'); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %Grafica la velocidad real y la estimada%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure(7); plot(t,x(:,2)*60/(2*pi),'b'); hold on; plot(t,xestmatriz(:,2)*60/(2*pi),'g'); plot(t,xestmatriz(:,2)*60/(2*pi)-2*sqrt(P_diagonal(:,2))*60/(2*pi),'-y'); %Grafica rango donde debiera estar el estado real plot(t,xestmatriz(:,2)*60/(2*pi)+2*sqrt(P_diagonal(:,2))*60/(2*pi),'-y'); %Grafica rango donde debiera estar el estado real grid off; legend('Velocidad real','Velocidad estimada (Kalman,', 'Rango ubicación velocidad real (Kalman)',0); hold off; title('Velocidad v/s iteración'); xlabel(['Tiempo (s)']); ylabel('Velocidad (RPMs)]'); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %Grafica el error en la velocidad%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure(8); plot(t,x(:,2)*60/(2*pi)-xestmatriz(:,2)*60/(2*pi),'r'); hold on; plot(t,-2*sqrt(P_diagonal(:,2))*60/(2*pi),'-y'); %Grafica rango donde debiera estar el estado real con 95% de probabilidad plot(t,+2*sqrt(P_diagonal(:,2))*60/(2*pi),'-y'); %Grafica rango donde debiera estar el estado real con 95% de probabilidad hold off; grid off; title('Error velocidad v/s iteración') legend('Error velocidad', 'Rango ubicación velocidad real (Kalman)',0); xlabel(['Tiempo (s)']); ylabel('Error (RPMs)]'); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %Grafica las corrientes Ia reales y las estimadas%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure(9); plot(t,x(:,3),'b'); %'.' hold on; plot(t,salidareal(:,1),'r+'); plot(t,xestmatriz(:,3),'g'); plot(t,xestmatriz(:,3)-2*sqrt(P_diagonal(:,3)),'-y'); %Grafica rango donde debiera estar el estado real plot(t,xestmatriz(:,3)+2*sqrt(P_diagonal(:,3)),'-y'); %Grafica rango donde debiera estar el estado real 64 grid on; legend('Corriente real', 'Corriente Medida', 'Corriente estimada (Kalman)', 'Rango ubicación corriente real (Kalman)',0); title('Corriente Ia v/s tiempo'); xlabel(['Tiempo (s)']); ylabel('Corriente Ia [A]'); hold off; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %Grafica las corrientes Ib reales y las estimadas%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure(10); plot(t,x(:,4),'b'); %'.' hold on; plot(t,salidareal(:,2),'r+'); plot(t,xestmatriz(:,4),'g'); plot(t,xestmatriz(:,4)-2*sqrt(P_diagonal(:,4)),'-y'); %Grafica rango donde debiera estar el estado real plot(t,xestmatriz(:,4)+2*sqrt(P_diagonal(:,4)),'-y'); %Grafica rango donde debiera estar el estado real grid on; legend('Corriente real', 'Corriente Medida', 'Corriente estimada (Kalman)', 'Rango ubicación corriente real (Kalman)',0); title('Corriente Ib v/s tiempo'); xlabel(['Tiempo (s)']); ylabel('Corriente Ib [A]'); hold off; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %Grafica el error en la coriente Ia%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure(11); plot(t,x(:,3)-xestmatriz(:,3),'r'); hold on; plot(t,-2*sqrt(P_diagonal(:,3)),'-y'); %Grafica rango donde debiera estar el estado real plot(t,2*sqrt(P_diagonal(:,3)),'-y'); %Grafica rango donde debiera estar el estado real grid on; legend('Error en la estimación de la corriente', 'Rango ubicación corriente real (Kalman)',0); title('Error corriente Ia v/s tiempo'); xlabel(['Tiempo (s)']); ylabel('Error Ia [A]'); hold off; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %Grafica el error en la coriente Ib%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure(12); plot(t,x(:,4)-xestmatriz(:,4),'r'); hold on; plot(t,-2*sqrt(P_diagonal(:,4)),'-y'); %Grafica rango donde debiera estar el estado real 65 plot(t,2*sqrt(P_diagonal(:,4)),'-y'); estado real grid on; %Grafica rango donde debiera estar el legend('Error en la estimación de la corriente', 'Rango ubicación corriente real (Kalman)',0); title('Error corriente Ib v/s tiempo'); xlabel(['Tiempo (s)']); ylabel('Error Ib [A]'); hold off; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %Grafica la estimación del torque desarrollado%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure(13); plot(t,Tm,'k',t,Tm_est,'-m',t,Tcarga,'.c'); grid on; legend('Torque real','Torque estimado', 'Torque carga',0); title('Estimación del torque desarrollado v/s tiempo'); xlabel(['Tiempo (s)']); ylabel('Torque Tm [N*m]'); hold off; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %Grafica el error en la estimación del torque%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure(14); plot(t,Tm-Tm_est,'b'); hold on; grid on; title('Error en la estimación del torque desarrollado v/s tiempo'); xlabel(['Tiempo (s)']); ylabel('Error estimación Tm [N*m]'); hold off; A.8 ejecucion_EKF2_codigo.m Este código implementa el filtro de Kalman Extendido, considerando al torque de carga como un parámetro a estimar. Utiliza la función kalman_iteracion_v2.m. %Versión del Filtro de Kalman Extendido Discreto (Versión 2.5) %para el motor stepper, sin utilizar el TORQUE DE CARGA!! (para verdadera estimación) disp('Inicio algoritmo Filtro de Kalman Extendido'); disp(' '); %Antes, debe haber ejecutado el Modelo del motor paso-a-paso %que genera un vector columna 't' con los tiempos de muestra %y una matrix 'x' con los estados puestos en fila, para cada t: %x=[x1;x2;x3;x4]=[theta;omega;i1;i2] %(cada fila es un tiempo distinto) %tambien debe existir la variable delta_t %----------------------------------SISTEMA--------------------------------- 66 %El estado fue calculado previamente y existe en el workplace %Necesito los parámetros del sistema estimado, pero son calculados en cada %iteración salvo por B y C B=[0 0; 0 0; 1/L 0; 0 1/L]; C=[0 0 1 0; 0 0 0 1]; %(Definicion del estado: x=[theta, w, ia, ib] ) %%%%Extención matrices para estado extendido con torque%%%% %%%La matriz B del sistema linealizado extendido%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% B=[B; 0 0] %no se ocupa %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%La matriz C del sistema linealizado extendido%%%%%%%% C=[C, [0;0]] %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %-------------------------------------------------------------------------%Parametros simulación: %tiempo_simulacion=t(length(t)) %delta_t muestras=length(t); %tiempo de simulacion [s] %Tiempo de muestreo[s] %---------------------------FILTRO DE KALMAN------------------------------%-----------------Parámetros del Filtro-----------------------------------P0=diag([1, 1, 1*1, 1*1, 10]) %Covarianza del error del estado inicial %sigmaplanta=10 %desviacion del modelo (ruido) (en distintas unidades) para covarianza de ruido del proceso Q %Q=eye(4) %matriz de covarianza del ruido del sistema %Q=diag([sigmaplanta^2, sigmaplanta^2, sigmaplanta^2, sigmaplanta^2]) Q=diag([1e-3, 1e-3, 1, 1, 1e-3]) %Q=diag([1e-3, 1e-3, 1, 1, 1e-3]) (funca para torque directo) sigmamedicion=Vnom/R*0.007 mediciones(A) %desviacion estandar (ruido) de las %se relaciona con la covarianza de ruido de medición Rcov %sigmamedicion=sqrt(0.1) %0.007 es el ejemplo de precicion del transductor de corriente LEM LTS 15-NP %Rcov=eye(2) %matriz de covarianza del ruido de la medición %Rcov=diag([sigmamedicion^2 sigmamedicion^2]) Rcov=diag([0.1 0.1]) %-------------------------------------------------------------------------%Variables del filtro xest=[]; %estimación S=[]; % covarianza de innovación P=[]; % covarianza del sistema %Definicion del ESTADO DEL ESTIMADOR: x=[theta, w, ia, ib, Torque] %Estimación Inicial disp('Estimación inicial:'); xest=[0,0,0.25,0,0] %Inicialización de variables filtro %P0=B*Q*B' % Error de covarianza inicial (Jan PERSSON metodo 1) %P0=[12*12 0;0 15*15]% Error de covarianza inicial (Jan PERSSON metodo 2) P=P0; 67 %---------------------------------EXTRAS----------------------------------xestmatriz=[]; xestmatriz(1,:)=xest; P_diagonal=[]; %para grabar los valores en la diagonal de P (autovarianzas) P_diagonal(1,:)=diag(P); salidareal=[]; %Para graficar las mediciones salidareal=[salidareal ; ( ([0 0 1 0; 0 0 0 1]*xo')+[randn*sigmamedicion; randn*sigmamedicion] )' ]; %------------------------------ITERACIONES--------------------------------for k=2:muestras %%%%%%%%% El sistema real%%%%%%%%%%%%%%%%%%%%% i1=x(k,3); i2=x(k,4); y=[i1;i2]+[randn*sigmamedicion; randn*sigmamedicion]; %Medición del sistema, agrego ruido salidareal=[salidareal ;y']; %Guardo la medición u=(genera_voltajes(t(k-1))) ; es la entrada para el estado anterior en (k-1) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %U %%%%%%%%% El sistema estimado%%%%%%%%%%%%%%%%% pos_est=xest(1); %esta es la estimación de la iteración anterior w_est=xest(2); ia_est=xest(3); ib_est=xest(4); Va=u(1); Vb=u(2); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%% Linealización y discretización%%%%%%%% %%Cálculo del jacobiano Dg%%%% Dg=[]; Dg=[0 1 0 0]; Dg21= -(N/2)*(Km/J)*( ia_est*cos((N/2)*pos_est)+ib_est*sin((N/2)*pos_est) ) - 4*(N/2)*Kd*cos(4*(N/2)*pos_est)/J; %(inv) Dg21= -(N/2)*(Km/J)*( ia_est*sin((N/2)*pos_est)+ib_est*cos((N/2)*pos_est) ) 4*(N/2)*Kd*sin(4*(N/2)*pos_est)/J; Dg=[Dg ; Dg21, -B_roce/J, -(Km/J)*sin((N/2)*pos_est), (Km/J)*cos((N/2)*pos_est) ]; Dg=[Dg ; ((N/2)*w_est*Km/L)*cos((N/2)*pos_est), (Km/L)*sin((N/2)*pos_est), -R/L, 0 ]; Dg=[Dg ; ((N/2)*w_est*Km/L)*sin((N/2)*pos_est), (Km/L)*cos((N/2)*pos_est), 0 ,-R/L]; %Extención para el torque %Dg=[Dg [0; -1/J; 0 ; 0]];%Extendemos la matriz para incluir la variable de estado Torque %Dg=[Dg; 0 0 0 0 1]; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%Cálculo función f%%%%% func_ es xpunto' 68 func_f=[]; %theta punto es omega func_f(1)=xest(2); % omega punto es alfa: (acá se puede incluir o no el término con Kd) %func_f(2)=(-Km*xest(3)*sin(xest(1)*N/2)+Km*xest(4)*cos(xest(1)*N/2)B_roce*xest(2)-(torque_carga(t(k-1))))/J; %OJOOOO con lo anterior!!!!! no deberia conocer el valor de la carga!!! func_f(2)=(-Km*xest(3)*sin(xest(1)*N/2)+Km*xest(4)*cos(xest(1)*N/2) Kd*sin(4*(N/2)*xest(1)) -B_roce*xest(2) - TcargaRef)/J; %ia punto: func_f(3)=(Va-R*xest(3)+Km*xest(2)*sin((N/2)*xest(1)))/L; %ib punto: func_f(4)=(Vb-R*xest(4)-Km*xest(2)*cos((N/2)*xest(1)))/L; %Torque punto: %func_f(5)=0; %equivale a extender el B %%%%%%%%%%%%%%%% %%%u_asterisco%% u_asterisco=(func_f)' - Dg*(xest(1:4)') ; %vector (columna) de entrada u* u_asterisco=[u_asterisco;0]; %%%%%%%%%%%%%%% %%%La matriz A del sistema discretizado%%%%%%% A=(eye(4)+Dg*delta_t); %5 matriz A del sistema linealizado y discretizado %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %La A=[A [0; -1/J; 0 ; 0]];%Extendemos la matriz para incluir la variable de estado Torque A=[A; 0 0 0 0 1]; %test %%%%%%%%% Filtro de Kalman%%%%%%%%%%%%%%%%%%%% (llama al algoritmo clásico) temporal=kalman_iteracion_v2(xest',P,y,u_asterisco,A, (eye(5)*delta_t),C,Q,Rcov); xest=(temporal(:,1))'; %porque salida de kalman_iteracion_v2 es [xest, P]; P=[temporal(:,2) temporal(:,3) temporal(:,4) temporal(:,5) temporal(:,6)]; %porque salida de kalman_iteracion_v2 es [xest, P]; %%%%%%%%extras al sistema%%%%%%%%%%%%%%%%%%%%% (para grabar valores) xestmatriz(k,:)=xest; %guarda los valores de estimaciones para graficar xest P_diagonal(k,:)=diag(P); %guarda la diagonal para graficar P if(k==(round(muestras*0.25))) disp('Kalman:25%') end if(k==(round(muestras*0.5))) disp('Kalman:50%') end if(k==(round(muestras*0.75))) 69 disp('Kalman:75%') end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% end disp('Fin algoritmo Filtro de Kalman Extendido'); disp(' '); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %Estimación del torque desarrollado por el motor%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Tm_est=Km*(-xestmatriz(:,3).*sin((N/2)*xestmatriz(:,1)) + xestmatriz(:,4).*cos((N/2)*xestmatriz(:,1))) Kd*sin((4*N/2)*xestmatriz(:,1)); %El torque real: Tm=Km*(-x(:,3).*sin((N/2)*x(:,1)) + x(:,4).*cos((N/2)*x(:,1))) Kd*sin((4*N/2)*x(:,1)); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %Grafica la posición real y la estimada%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure(5); plot(t,x(:,1)/(2*pi),'b'); hold on; %plot(salidareal,'r+'); plot(t,xestmatriz(:,1)/(2*pi),'--g'); %plot(t,xestmatriz(:,1)/(2*pi)-2*sqrt(P_diagonal(:,1))/(2*pi),'-y'); %Grafica rango donde debiera estar el estado real %plot(t,xestmatriz(:,1)/(2*pi)+2*sqrt(P_diagonal(:,1))/(2*pi),'-y'); %Grafica rango donde debiera estar el estado real grid off; legend('Posición real', 'Posición estimada (Kalman)',0); %legend('Posición real', 'Posición Medida','Posición estimada (Kalman)', 'Rango ubicación posición real (Kalman)',0); title('Posición v/s tiempo'); xlabel(['Tiempo (s)']); ylabel('Posición Theta (en Vueltas)'); hold off; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %Grafica el error en la posición%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure(6); plot(t,x(:,1)/(2*pi)-xestmatriz(:,1)/(2*pi),'r'); hold on; %plot(t,-2*sqrt(P_diagonal(:,1))/(2*pi),'-y'); %Grafica rango donde debiera estar el estado real con 95% de probabilidad %plot(t,+2*sqrt(P_diagonal(:,1))/(2*pi),'-y'); %Grafica rango donde debiera estar el estado real con 95% de probabilidad hold off; grid off; title('Error posición v/s iteración'); legend('Error posición', 'Rango ubicación posición real (Kalman)',0); xlabel(['Tiempo (s)']); ylabel('Error (en Vueltas)'); 70 %%{ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %Grafica la velocidad real y la estimada%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure(7); plot(t,x(:,2)*60/(2*pi),'b'); hold on; plot(t,xestmatriz(:,2)*60/(2*pi),'g'); %plot(t,xestmatriz(:,2)*60/(2*pi)-2*sqrt(P_diagonal(:,2))*60/(2*pi),'-y'); %Grafica rango donde debiera estar el estado real %plot(t,xestmatriz(:,2)*60/(2*pi)+2*sqrt(P_diagonal(:,2))*60/(2*pi),'-y'); %Grafica rango donde debiera estar el estado real grid off; legend('Velocidad real','Velocidad estimada (Kalman,', 'Rango ubicación velocidad real (Kalman)',0); hold off; title('Velocidad v/s iteración'); xlabel(['Tiempo (s)']); ylabel('Velocidad (RPMs)]'); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %Grafica el error en la velocidad%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure(8); plot(t,x(:,2)*60/(2*pi)-xestmatriz(:,2)*60/(2*pi),'r'); hold on; %plot(t,-2*sqrt(P_diagonal(:,2))*60/(2*pi),'-y'); %Grafica rango donde debiera estar el estado real con 95% de probabilidad %plot(t,+2*sqrt(P_diagonal(:,2))*60/(2*pi),'-y'); %Grafica rango donde debiera estar el estado real con 95% de probabilidad hold off; grid off; title('Error velocidad v/s iteración') legend('Error velocidad', 'Rango ubicación velocidad real (Kalman)',0); xlabel(['Tiempo (s)']); ylabel('Error (RPMs)]'); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %} %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %Grafica las corrientes Ia reales y las estimadas%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure(9); plot(t,x(:,3),'b'); %'.' hold on; plot(t,salidareal(:,1),'r+'); plot(t,xestmatriz(:,3),'g'); plot(t,xestmatriz(:,3)-2*sqrt(P_diagonal(:,3)),'-y'); %Grafica rango donde debiera estar el estado real plot(t,xestmatriz(:,3)+2*sqrt(P_diagonal(:,3)),'-y'); %Grafica rango donde debiera estar el estado real grid off; legend('Corriente real', 'Corriente Medida', 'Corriente estimada (Kalman)', 'Rango ubicación corriente real (Kalman)',0); title('Corriente Ia v/s tiempo'); xlabel(['Tiempo (s)']); ylabel('Corriente Ia [A]'); hold off; 71 %{ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %Grafica las corrientes Ib reales y las estimadas%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure(10); plot(t,x(:,4),'b'); %'.' hold on; plot(t,salidareal(:,2),'r+'); plot(t,xestmatriz(:,4),'g'); plot(t,xestmatriz(:,4)-2*sqrt(P_diagonal(:,4)),'-y'); %Grafica rango donde debiera estar el estado real plot(t,xestmatriz(:,4)+2*sqrt(P_diagonal(:,4)),'-y'); %Grafica rango donde debiera estar el estado real grid off; legend('Corriente real', 'Corriente Medida', 'Corriente estimada (Kalman)', 'Rango ubicación corriente real (Kalman)',0); title('Corriente Ib v/s tiempo'); xlabel(['Tiempo (s)']); ylabel('Corriente Ib [A]'); hold off; %} %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %Grafica el error en la coriente Ia%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure(11); plot(t,x(:,3)-xestmatriz(:,3),'r'); hold on; %plot(t,-2*sqrt(P_diagonal(:,3)),'-y'); %Grafica rango donde debiera estar el estado real %plot(t,2*sqrt(P_diagonal(:,3)),'-y'); %Grafica rango donde debiera estar el estado real grid off; legend('Error en la estimación de la corriente', 'Rango ubicación corriente real (Kalman)',0); title('Error corriente Ia v/s tiempo'); xlabel(['Tiempo (s)']); ylabel('Error Ia [A]'); hold off; %{ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %Grafica el error en la coriente Ib%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure(12); plot(t,x(:,4)-xestmatriz(:,4),'r'); hold on; plot(t,-2*sqrt(P_diagonal(:,4)),'-y'); %Grafica rango donde debiera estar el estado real plot(t,2*sqrt(P_diagonal(:,4)),'-y'); %Grafica rango donde debiera estar el estado real grid off; legend('Error en la estimación de la corriente', 'Rango ubicación corriente real (Kalman)',0); title('Error corriente Ib v/s tiempo'); xlabel(['Tiempo (s)']); ylabel('Error Ib [A]'); 72 hold off; %} %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %Grafica la estimación del torque desarrollado%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure(13); plot(t,Tm_est,'g',t,Tm,'b',t,Tcarga,'c'); hold on; grid off; legend('Torque estimado','Torque motor', 'Torque carga',0); title('Estimación "directa" del torque desarrollado v/s tiempo'); xlabel(['Tiempo (s)']); ylabel('Torque Tm [N*m]'); hold off; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %Grafica el error en la estimación del torque%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure(14); plot(t,Tm-Tm_est,'b'); hold on; grid off; title('Error en la estimación "directa" del torque desarrollado v/s tiempo'); xlabel(['Tiempo (s)']); ylabel('Error estimación Tm [N*m]'); hold off; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %Grafica el torque carga, real y el estimado por kalman%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure(15); plot(t,Tm ,'b', t, Tcarga, 'c'); hold on; %plot(t,xestmatriz(:,5) +TcargaRef,'g'); plot(t,xestmatriz(:,5) ,'g'); %plot(t,xestmatriz(:,5) +TcargaRef -2*sqrt(P_diagonal(:,5)),'-y'); rango donde debiera estar el estado real %plot(t,xestmatriz(:,5) +TcargaRef +2*sqrt(P_diagonal(:,5)),'-y'); rango donde debiera estar el estado real grid off; %Grafica %Grafica legend('Torque real','Torque carga', 'Carga estimada (Kalman)' ,0); title('Torque de carga v/s tiempo'); xlabel(['Tiempo (s)']); ylabel('Torque [N*m]'); hold off; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %Grafica el error en el torque de la carga%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure(16); %plot(t,Tcarga - xestmatriz(:,5) -TcargaRef,'b'); plot(t,Tcarga - xestmatriz(:,5) ,'b'); hold on; %plot(t,-2*sqrt(P_diagonal(:,5)),'-y'); %Grafica rango donde debiera estar el estado real con 95% de probabilidad 73 %plot(t,+2*sqrt(P_diagonal(:,5)),'-y'); debiera estar el estado real con 95% de probabilidad hold off; grid off; %Grafica rango donde title('Error torque carga v/s iteración') legend('Error carga', 'Rango ubicación torque real (Kalman)',0); xlabel(['Tiempo (s)']); ylabel('Error (N*m)]'); 74 Sin Cos cos sin 1 0.05 Km [N*m/A] Voltaje A 1 s v elocidad A Nº de vueltas -KKm*cos() 1 Km*sin() -K- 1/J[N*m*s^2/rad] -> Aceleración Salida (posición) 1 s dIa/dt Torque Motor Ia*Km*sin() -> Corriente A -C- 75 3 1/L[H] Salida (corrientes) 1 s dIb/dt -> Corriente B Salida (velocidad) A RPMs -K- 2 Ib*Km*cos() -K- Rb [ohms] -K- w*Km*cos() 2 Voltaje B 1 s -C- velocidad -> angulo -KRa [ohms] Torque Carga [NM] aceleracion -> velocidad B [N*m*s/rad] -> friccion viscosa Anexo B - Diagramas Simulink Pares de polos B.1 sistema_stepper_rbl.mdl 5 ángulo A continuación se muestra el diagrama de bloques del el modelo del motor de 2 fases, implementado en Simulink: w*km*sin()