Translate

viernes, 1 de febrero de 2013

Controlador PID para posición de un motor DC

En el tema anterior construimos una gráficas mostrando la relación entre el input (% PWM) aplicado y el resultado (velocidad) obtenido por el motor. Podríamos usar esa información para controlar nuestro motor. Si queremos que gire con una cierta velocidad, podríamos ver que entrada (PWM) precisamos y aplicarla. Sin embargo ya comentamos que dicha relación cambia por numerosos factores, por lo que nunca estaríamos seguros de alcanzar la velocidad deseada.

Técnicamente esta situación se conoce como un control de bucle abierto, al no tener un feedback que nos informe de lo que realmente está haciendo el motor. De hecho disponemos de dicho feedback (la info de los encoders en cuadratura) pero hasta ahora no la estamos usando.

En esta entrada vamos a "cerrar" el bucle, usando el feedback de que disponemos para controlar la posición de un  motor. La entrada suministrada por el usuario no será un PWM (de efectos inciertos) sino un objetivo en términos de la velocidad o posición deseada del motor. El sistema, en función del feedback recibido, variará adecuadamente su variable de control (en este caso el ciclo ON del PWM) para conseguir el objetivo dado. Usaremos un sencillo controlador PID para la implementación.

Al igual que en el tema anterior, acompañaremos al código en C del microcontrolador con un programa en MATLAB para poder mostrar de forma gráfica los movimientos del motor.

video


Archivos de código asociado a esta entrada: 
          PID_pos.c  monitor_PID.m (MATLAB)



-----------------------------------------------------------------------------------------------------------

Controlador PID de posición.

En aplicaciones de control el primer paso es identificar la variable que queremos controlar y la variable que podemos modificar. En un ejemplo típico, podríamos querer controlar la temperatura de un acuario y para ello podemos actuar sobre el tiempo que un calentador está encendido.

En nuestro caso la variable a controlar será la posición del motor y la variable de actuación será el duty del PWM. Esencialmente estamos interesados en convertir nuestro motor DC en un servomotor, al que le podamos decir a que posición queremos que vaya.

La idea de cualquier sistema de control es poder calcular una discrepancia (error) entre el objetivo (posición deseada, introducida por el usuario) y la realidad (posición del motor obtenida a través de los encoders). Dicho error se usará como feedback para modificar la variable de control (PWM en nuestro caso).

Un controlador PID es un tipo de control muy usado en la práctica por su sencillez, dado que no requiere una caracterización completa del sistema. Se basa en combinar 3 términos (Proporcional, Integral y Diferencial)  relacionados con el error con unas constantes que se pueden determinar a través de un proceso de prueba y error.

En nuestro caso, siendo Pref la posición deseada (target u objetivo) y siendo pos (determinada por los encoders) la posición real del motor, definiremos el error en un momento dado como
                 

A partir de este error, modificaremos nuestra variable de actuación en base a la siguiente expresión:
Por el momento podemos considerar que pid es simplemente un valor que, en caso de ser positivo, indica que el motor debe girar hacia delante y si es negativo, hacia atrás. Nosotros nos ocuparíamos de convertirlo a un valor p.e. entre 0 y 1023 como vimos en el tema anterior para aplicarlo al PWM (o entre -1023 y 1023 si usamos el montaje A).

En la fórmula, el primer término es proporcional al error, el segundo proporcional a su integral y el tercero va con su derivada. 

La idea es que el primer término trata de reducir el error actual. Si el error es positivo (no hemos llegado a Pref), pid es positivo, por lo que aumenta la posición y se reduce el error. Si el error es negativo (nos hemos pasado), se invierte el duty y nos movemos en sentido contrario, acercándonos de nuevo al objetivo.

El segundo término refleja la influencia del "pasado" del error: nuestra actuación no será la misma si puntualmente tenemos un cierto error  que si llevamos un "rato" largo con dicho error. En el segundo caso la actuación será más enérgica.

Finalmente el tercer término refleja el "futuro" del error (su tendencia, capturada en su derivada). Imaginad que estamos a 5 unidades de nuestra posición deseada, o lo que es lo mismo, tenemos un error e=5. No es lo mismo si nos estamos acercando al objetivo (se está reduciendo el error, derivada negativa) que si nos estamos alejando (derivada positiva, el error está aumentando). En el primer caso a lo mejor no hay que hacer casi nada (la propia inercia del movimiento nos lleva al objetivo, mientras que en el otro caso está claro que hay que corregir la situación.

En nuestro caso en vez del continuo antes descrito tendremos una implementación discreta. Cada cierto tiempo (en la rutina del servo de la que hemos hablado en temas anteriores) se mide la posición actual (encoders) y se calcula un término de error:
Para calcular el término de control, el error instantáneo se traduce de forma natural en el error medido en el instante n, pero para la parte integral y derivativa tendremos que hacer aproximaciones numéricas:

  • La integral se substituye por una suma de valores anteriores del error. Además se limita en el tiempo, sumando los últimos N errores, en vez del historial completo de errores. Formalmente tendríamos que añadir un término h (el intervalo de tiempo entre muestras) pero dicho término (constante) puede ser absorbido por la constante de integración Ki.
  • En cuanto a la estimación numérica de la derivada, lo más inmediato es substituirla por la diferencia entre errores sucesivos, aunque pueden usarse estimaciones más exactas. De nuevo, el factor h que aparece puede entrar dentro de la constante Kd, por lo que no es necesario introducirlo explícitamente:  
En su variante discreta el término de control a calcular para actuar sobre el PWM del motor será por tanto:
Indicar por último que no es necesario que la frecuencia de toma de medidas sea la misma que la frecuencia de actuación del servo. Es posible, p.e. tomar medidas con una frecuencia de 100 Hz pero solo actuar sobre el motor 20 veces por segundo (20 Hz). En estos ejemplos por simplicidad usaremos la misma frecuencia para ambos, por lo que tras tomar cada medida, calcularemos el término de control y modificaremos el ciclo ON del PWM.

Implementación (PID_pos.c)

Pasemos a la implementación. La mayor parte del código ya la hemos visto en las entradas anteriores. Por ejemplo, la parte de la interrupción de alta prioridad INT0 para contar los pulsos de los encoders es la misma, así como el uso de la interrupción del TMR2 para la rutina del servo.

La diferencia es que la rutina del servo antes sólo actualizaba la posición del motor (a partir de la lectura de los encoders). Ahora, a partir de dicha posición y del objetivo fijado tendremos que calcular el término de actuación anterior (pid) y traducirlo a nuestro esquema particular. Por ejemplo, un pid = 0 indica que todo esta bien y no hay que hacer nada. Eso equivale a ordenar un PWM de 512 (50%) si hemos elegido un montaje de tipo B (ver entrada anterior).

Veamos con más detalle los añadidos a la rutina del servo (interrupción TMR2)

if (TMR2_flag)
 {
  cont_servo++;
  if ((cont_servo&0x0F)==0)
   {
    temp=quad; vel=temp-last_quad;    // delta = delta counter
    last_quad=temp;     
    if (vel>128) vel-=256;          // correct for rollover
    else if (vel<-128) vel+=256; 
    pos=pos+vel;                      // increment position
      
    error=Pref-pos;   // Current error
    
    if (err_idx==0) last=past[NPAST-1]; else last=past[err_idx-1];
    error_der = (error - last);  // Derivative error


    // Acumulated error
    error_int-=past[err_idx]; error_int+=error;
    past[err_idx]=error;
    err_idx++; err_idx&=(NPAST-1);

    compute_pid();  // combine PID terms

   
    create_msg(); send_msg(msg,12); // Send data through serial port

   }
  TMR2_flag=0; return;
 }


La primera parte es idéntica a la que ya conocemos. La rutina del servo se ejecuta cada cierto número de interrupciones (en este caso, 16, que junto con un POSTscaler de 16 para TMR2 nos da una frecuencia de unos 75 Hz para el servo). Para ello usamos el contador cont_servo.

La parte de calcular el incremento de posición es también idéntica. Corregimos posibles rollovers de la variable quad e incrementamos la posición.  A partir de aquí empezamos a hacer cosas nuevas.

A partir de un objetivo establecido para la posición (Pref) se calcula:
  • Error instantáneo (error) para la posición de ese momento (error instantáneo).
  • Estimación de la derivada del error (error_der) restando errores sucesivos.
  • Error integral acumulado (error_int).
Para el cálculo del error integral o acumulado se guardan los NPAST últimos errores en un buffer circular donde van siendo machacados según llegan datos nuevos. El error integral es simplemente la suma de los contenidos del buffer en un momento dado. Para optimizar su cálculo, lo que hacemos es restar de la suma total el error más antiguo (antes de machacarlo) y sumar el nuevo error (que machaca al más antiguo).  Así evitamos tener que hacer una suma de todos los datos cada vez. Tanto el buffer como su suma total se inicializan a 0 al empezar.
 
    // Error integral
    error_int-=past[err_idx]; error_int+=error;
    past[err_idx]=error;
    err_idx++; err_idx&=(NPAST-1);

Tras almacenar el error más reciente se incrementa el cursor (err_idx) del buffer circular de antiguos errores (past). En este ejemplo estamos usando un tamaño NPAST=32 para el buffer del "pasado". Aproximadamente conservamos algo menos de ½ segundo (32 muestras a 75 Hz) en nuestro buffer.

Finalmente, una vez conocidos el error instantáneo, error integral (suma del buffer del pasado) y error derivativo  tenemos todo lo necesario para saber como actuar. La rutina compute_pid() hace exactamente eso: combina los valores de los tres términos con las tres constantes (guardadas en un array K global), obtiene el término de control y lo convierte a un valor del "duty cicle" del PWM que programamos en el correspondiente registro del PIC: 

void compute_pid(void)
{
 float ee,ed,ei;
 float pid;
 
 ee = error;  // Current error
 ed = error_der;   // Derivative error
 ei=  error_int/NPAST;  // Integral error

 pid = K[0]*ee + K[1]*ei + K[2]*ed;  // P,I,D terms
 
 if (pid>=1.0) pid=1.0; else if (pid<=-1.0) pid=-1.0;  // pid in -1..1

 pid =512+500*pid;
 set_pwm1((uint16)pid);

 return;
}

De nuevo podemos elegir entre las distintas opciones discutidas en el tema anterior sobre como conectar las líneas de control al driver del motor. Como hemos usado el motor Maxon GM-20 para las pruebas hemos optado por el montaje (B) que mostraba un comportamiento muy lineal entre PWM y velocidad.  Por lo tanto el valor de control, entre -1 y 1 se translada al intervalo [0, 1023], de forma que un valor de control 0 vaya a parar a 512 (50% PWM), que corresponde a mantener el motor en reposo (nominalmente).

Como vimos en el tema anterior diferentes combinaciones de motores/drivers pueden mostrar peculiaridades propias que hagan necesarias algunas correcciones a la traducción entre variable de control (-1,1) y PWM. Por ejemplo, para esta combinación de driver L293D y motor GM20 detectamos una pequeña zona muerta ("deadband") en la zona entre PWM=500 y 520. Podríamos compensarla en "software" de la siguiente forma:

 pid=pid*500;
 if (pid>0) pid+=520; else if (pid<0) pid+=500; else pid=512;

De esta forma, el mínimo PWM que se adjudica a un pid no nulo es de 520, lo que asegura que el motor se moverá algo. De la otra forma un error pequeño podría darnos un PWM de p.e. 515. Dicho PWM, aunque nominalmente debería mover el motor, en la práctica no tiene ningún efecto, por lo que el error no se reduciría.

Finalmente, al final del proceso vemos un par de rutinas create_msg y send_msg(). Lo que hacen estas rutinas es construir un pequeño paquete con los datos más relevantes y enviarlo por el puerto serie. El paquete usado sigue el esquema explicado en el tema anterior: empieza con el carácter 'A', sigue con unos cuantos datos (posición, velocidad, objetivo y PWM duty) y termina con un contador de mensajes y el carácter 'Z' cerrando el paquete.

void create_msg(void)
{
 int16 temp;
 uint8 k,*ptr;

 msg[0]='A';
 ptr=(uint8*)&pos; for(k=0;k<4;k++) msg[k+1]=ptr[k]; 
 msg[5]=vel;
 temp=(int16)Pref;
 ptr=(uint8*)&temp; for(k=0;k<2;k++) msg[k+6]=ptr[k]; 
 ptr=(uint8*)&duty; for(k=0;k<2;k++) msg[k+8]=ptr[k]; 
 msg[10]=msg_cont; msg_cont++;
 msg[11]='Z';
}

void send_msg(uint8* msg,uint8 n)
{
 uint8 k;
 for(k=0;k<n;k++) { tx_buf[tx_next]=msg[k]; inc(tx_next);}
 enable_TX_int;
}

Tras crear el paquete la segunda rutina lo manda por el puerto serie. Como aquí podríamos estar mandando un volumen grande de datos hemos usado el enfoque de transmisión de datos a través de la UART usando un buffer de transmisión e interrupciones. Por lo tanto la segunda rutina no hace más que copiar los bytes del mensaje al buffer de salida y activar la interrupción TX. El código de dicha interrupción irá vaciando dicho buffer. En previsión de si aumentamos el número de datos o la frecuencia con la que los mandamos configuraremos el puerto serie a 115200 baudios.


Interfaz con el usuario

Para comprobar nuestro código de control podríamos haber hecho algo similar a lo que hicimos para el caso de variar el PWM: implementar un sencillo menú donde, a través del puerto serie, el usuario pudiera dar ordenes del tipo de: ve a la posición 2000, vuelve a la posición 0, etc.

Sin embargo, como se trata de un programa ilustrativo, y nuestro objetivo en este entrada es entender un poco mejor como funciona un controlador PID, lo que he hecho es que el usuario puede modificar las constantes Kp, Kd y Ki desde el PC (a través del programa de MATLAB monitor_PID), de forma que pueda tener una idea del proceso de ajustar los parámetros de un PID.

Dentro del programa en el microcontrolador,  partimos de un objetivo Pref=0 como "target" para la posición. Cada cierto tiempo (2/3 segundos), dicho objetivo cambia, alternando entre 0 y 500 (500 ticks con este motor en modo 2X corresponden aproximadamente a 1/4 de vuelta) . En un principio las tres constantes Kp, Ki y Kd están a 0, por lo que a pesar de los posibles errores no estamos actuando sobre el motor. 

A través de los botones de la GUI de MATLAB podemos subir, bajar o resetear dichas constantes. Así podemos explorar el efecto de los diferentes términos de un controlador PID en un problema sencillo pero realista.

El siguiente screencast muestra el manejo de la interfaz de MATLAB y el ajuste de las constantes del PID hasta obtener un resultado aceptable. Remarcar que no siempre es necesario usar los tres términos del PID. En este caso, p.e. obtenemos un buen resultado sólo con la parte proporcional y derivativa. 



video









22 comentarios:

  1. Espectacular, sos uin maestro es lo que estaba buscando, te voy a seguir desde ahora, espero que tambien subas algo de lo que haces con matlab. Saludos de argentina capo.

    ResponderEliminar
  2. Es posible emplear las instrucciones de c18 en ccs??

    ResponderEliminar
    Respuestas
    1. Las funciones de las librerías de uno u otro compilador serán diferentes por lo que el código no es inmediatamente portable. Sin embargo, en general, los compiladores tienen una funcionalidad similar, por lo que lo normal es que exista una función equivalente en el otro compilador.

      El enfoque que trato de darle a estas páginas es presentar un enfoque que intente explicar como están funcionando por debajo las cosas, a nivel de los registros del PIC. De esta forma nos será sencillo portar nuestros proyectos a otros compiladores. O incluso a otros micros, ya que los conceptos básicos son comunes a diferentes familias de micros: todos tienen interrupciones, timers, ADC, algún tipo de PWM, etc.

      Antonio

      Eliminar
    2. Gracias, la verdad la info presentada aqui es muy util y los conceptos son bien entendibles, una pregunta ¿sera que esto se pueda aplicar para controlar algo como un pendulo invertido?

      Puesto que en lo que tengo entendido el pendulo invertido se basa en que el motor actue en base ala señal que le mande el encoder que seria la del angulo y asi intentar estabilizar la varilla, no se si estoy mal en ese pensar. Saludos desde México

      Eliminar
    3. Este comentario ha sido eliminado por el autor.

      Eliminar
  3. es cierto sera posible aplicar esto para el control de un pendulo inverso???

    ResponderEliminar
    Respuestas
    1. Si que se podría usar para controlar un péndulo invertido.
      Lo que sucede es que si el péndulo a controlar está directamente fijado al eje del motor no va a resultar muy vistoso (la varilla del péndulo estaría simplemente bloqueada en la posición correspondiente a la vertical).
      Lo que tendrías que hacer para un proyecto más interesante es dejar libre la varilla del péndulo, montada sobre un carro o similar que tuviera un motor que permita moverlo hacia adelante o atrás.

      Un sensor (puede ser un simple potenciometro conectado a la varilla o un acelerómetro) ) nos permitiría medir su desplazamiento respecto de la vertical. Dicha medida gobernaría un controlador PID aplicado al motor que intentase mantenerla vertical. La idea es que si se empieza a caerse hacia adelante muevas el carro (a través del motor) hacia adelante, intentando mantener el carro debajo del centro de gravedad.

      Un saludo,

      Anronio

      Eliminar
  4. me podrias mandar el circuito?

    ResponderEliminar
    Respuestas
    1. No tengo circuito. La mayoría de estos ejemplos están montados sobre una protoboard. El esquema está explicado en la entrada anterior, sobre Manejo de motores usando PWM.

      Antonio

      Eliminar
  5. buenos días desde colombia. agradeceria su comentario en mi duda. Estoy en este momento desarrollando un proyecto con unos motores paso a paso. de 1.8 grados. de paso. para cada paso que da debo hacer un sensado de luz. y determinar en que grado de los 180 posibles hay menor cantidad de luz. tengo el inconveniente de necesitar mas precisión ojala de 1 grado por paso. por lo que he optado por buscar información acerca de estos motores para ver si es posible realizar la misma función que un motor paso a paso de 1° de paso que no he conseguido. agradezco nuevamente su colaboración. gracias

    ResponderEliminar
    Respuestas
    1. Si lo que necesitas es más precisión en determinar la posición angular del mínimo a partir de una serie de medidas espaciadas 1.8º siempre puedes recurrir a interpolar tus datos.

      El proceso sería el siguiente:

      1. Determinar por donde anda el mínimo. Por ejemplo puedes usar el valor mínimo obtenido en tus medidas, separadas 1.8º

      2. Coger varios puntos (por ejemplo 3) que enmarquen el mínimo. Siguiendo con el ejemplo anterior usaríamos el que tiene el mínimo y 1 adicional a cada lado.

      3. Hallar el polinomio de grado 2 que pasa por los tres datos. Sencillo de calcular usando una tabla de diferencias finitas (ver wikipedia)

      4. Usar como dirección angular el mínimo del polinomio de interpolación.

      Para fijar ideas, supón que tienes las siguientes medidas:

      Angulo: Medida:
      x-3.6 38
      x-1.8 35
      x 30
      x+1.8 31
      x+3.6 34
      ...

      y 30 es el mínimo de tus medidas. Quieres determinar el mínimo con mayor precisión.
      Si calculas el polinomio de interpolación para los 3 puntos centrales (x-1.8, x y x+1.8) y lo derivas para hallar el mínimo tendrás que este se sucede a 0.6º a la derecha de la medida central, esto es en x+0.6º.

      Espero haberte servido de ayuda, Antonio.

      Eliminar
  6. no comprendo aún dos cosas, una es si la realimentación es interna en el micro??? y la otra es como juegas con el set point para variar la posición, en que pin va conectado el potenciometro para variar la posición???

    ResponderEliminar
    Respuestas
    1. Si. el feedback llega al micro a través de la lectura de los encoders en cuadratura. Dentro del programa se hacen los cálculos para reducir a cero el error.
      En cuanto a variar el setpoint en este proyecto, por ser de caracter ilustrativo, está preprogramado en el micro. Cada cierto tiempo alterno entre dos setpoints para ver como pasa el motor de uno a otro.
      Sería trivial poner un potenciometro en uno de los pines y usar el conversor AD para leer su valor que se convertiría en el setpoint deseado.

      Antonio.

      Eliminar
    2. Este comentario ha sido eliminado por el autor.

      Eliminar
    3. los enconder van conectados en RB0 y RB1, verdad??, que tipo de encoders usaste, por si acaso no fueron los TCST1103??

      Eliminar
  7. Este comentario ha sido eliminado por el autor.

    ResponderEliminar
  8. Efectivamente están en los pines RB0 y RB1 que tienen interrupciones asociadas INT0,INT1 por cambio de estado. Consulta la entrada sobre codificadores en cuadratura para más detalles.
    De hecho si solo vas a usar el encoder en modo 2X podrías usar RB0 (con INT0) y cualquier otro pin declarado como una entrada digital, ya que no sería necesario asociar una interrupción a la segunda línea.
    De esta forma, dado que este PIC tiene tres interrunciones INT0,1,2 podrías llegar a poder monitorizar 3 pares de encoders (tres motores).
    Respecto a los encoders son los que venían integrados en los motores usados.

    Antonio

    ResponderEliminar
  9. Hola, antes que nada, quiero felicitarte, están muy bien hechos los tutoriales. Te escribo por que quiero saber si es posible que expliques como hiciste la GUI en Matlab, ya que he buscado en tus tutoriales y no he encontrado esa parte, en todos únicamente das la GUI ya hecha, pero no explicas cómo hacerla. Espero que puedas realizar un tutorial sobre cómo realizar la comunicación serial con Matlab, ya que en lo personal lo he intentado desde Simulink y no he podido hacer la comunicación bidireccional, por que truena la simulación, hacer el envío o la recepción por separado sí lo he hecho, pero a la hora de juntar ambas simulaciones en una sola, parece que se genera un problema con el puerto, se ejecuta un rato y después la simulación se detiene.


    Recibe saludos, y nuevamente muchas felicidades.

    ResponderEliminar
    Respuestas
    1. Me apunto la sugerencia de documentar una GUI tipo en MATLAB con comunicación serie bidireccional.
      Yo no estoy usando Simulink, para la comunicación serie uso el comando serial de MATLAB para crear un objeto serie, al que luego puedo acceder para lectura y escritura.

      Antonio.

      Eliminar
    2. Hola Antonio, muchas gracias por responder tan rápido. Estaré atento para cuando publiques la explicación de la GUI.

      Recibe saludos.

      Eliminar
  10. Me podrias pasar el diagrama de implementacion para el Isis

    ResponderEliminar
  11. Hola, felicitaciones por el blog, está genial, quisiera saber si alguna vez has desarrollado algo de tus proyectos con metodos numéricos, gracias

    ResponderEliminar