====== Algoritmos PID para siguelíneas ======

<WRAP tip> **Controlador PID** (Proporcional, Integral, Diferencial) Un controlador PID es un mecanismo de control por realimentación ampliamente usado en sistemas de control. Este calcula la desviación o error entre un valor medido y un valor deseado, y ejecuta una acción para ajustar el proceso. </WRAP>

Esta entrada de la wiki es una introducción a los controladores PID con un ejemplo ¿sencillo? y una aplicación práctica directa: robots que siguen una línea pintada en el suelo.

===== Dos partes diferenciadas: sensor y actuador =====

Cuando en cualquier sistema -como nuestra casa- queremos mantener un parámetro bajo control -como la temperatura- necesitamos dos partes separadas y diferenciadas:

  * Un **sensor**  para conocer el valor y el error respecto de un objetivo.
  * Un **actuador**  capaz de hacer cambios en el sistema para corregir el error.

Además de esas dos partes físicas necesitamos **una estrategia de actuación**. La que vamos a contar aquí es una relativamente simple y eficiente: un controlador PID, pero //first things first//.

==== Medida del error: el sensor ====

Necesitamos conocer el estado de nuestro sistema, y además lo compararemos con nuestro objetivo. En nuestro caso ese objetivo es seguir una línea pintada en el suelo. En las competiciones típicas de robots siguelíneas tenemos una base blanca con una línea negra de 2 centímetros de ancho. Hay otras con la pista pintada de un color degradado ¡el algoritmo también vale para estas!

Para detectar la línea utilizamos **sensores de reflectancia**. Estos sensores emiten luz con un diodo (normalmente infrarrojo) y leen la luz rebotada con un sensor de luz (típicamente un fotodiodo o un fototransistor). Cuando la luz rebota sobre una superficie negra da una lectura diferente que cuando rebota sobre una superficie blanca et voilà. Hay que tener cuidado, a la medida de estos sensores le afecta la distancia a la superficie de rebote y la luz ambiental.

{{:guias:guias:pid:irsensor.jpg?400}}

(imágen de [[https://diyhacking.com/make-line-follower-robot/|https://diyhacking.com/make-line-follower-robot/]])

Para conocer la posición de la línea con respecto a nuestro robot no vamos a usar un único sensor, sino **un conjunto de sensores alineados**  (a veces aparecen en inglés como array). Leyéndolos todos podemos saber dónde está la línea. Podemos construirlo o comprarlo hecho. Lo primero nos hará aprender más cosas, lo segundo seguramente nos dé un diseño más compacto.

{{:guias:guias:pid:array_ir.jpg?400}}

Con el programa siguiente, que corresponde a un sensor de Pololu QTR-8A (analógico), podemos leer los ocho sensores y sacar la lectura en el monitor serie de Arduino.

<code>
#include <QTRSensors.h> // utiliza la librería del fabricante del sensor
QTRSensorsAnalog qtra((unsigned char[]) {6, 7, 8, 9, 0, 1, 2, 3 }, 8);
// define el número de sensores y los pines donde están conectados

unsigned int IR[8];

void setup() {
  Serial.begin(9600);
}

void loop()
{
  qtra.read(IR); // lee los sensores

  for (int i=0; i<8; i++)
  {
    Serial.print(IR[i]); // muestra los valores por el puerto serie en líneas
    Serial.print(" , ");
  }

  Serial.println();
}
</code>

{{:guias:guias:pid:centro.jpg}}

{{:guias:guias:pid:derecha.jpg}}

Una vez comprobado que las conexiones son correctas y que nuestros ocho sensores funcionan y son capaces de detectar si están sobre blanco o sobre negro, vamos a **juntar todo en una única medida**  de error.

==== Componer el error ====

Como el anillo único para gobernarlo todo, vamos a utilizar la lectura de los ocho sensores para tener en un único número una medida de la posición de la línea o más bien del error de centrado.

Dividiremos los sensores a la izquierda y a la derecha asignándoles valores positivos y negativos. Las medidas de los sensores más alejados del centro los multiplicaremos por valores -'pesos'- mayores, y sumaremos todos para tener ahora

{{:guias:guias:pid:pesos_sensores.jpg?500}}

<code>
  error = -4*IR[0] -3*IR[1] -2*IR[2] -IR[3] +IR[4] +2*IR[5] +3*IR[6] +4*IR[7];
</code>

Este valor conjunto será negativo cuando nos desviemos a un lado y positivo cuando nos desviemos al otro lado. Los resultados serán mayores al alejarnos de la línea y cuando estemos sobre ella serán cercanos a cero: ¡no hay error!

{{:guias:guias:pid:sensor_compuesto.gif}}

Para los más puristas: esto no da un resultado estrictamente lineal, pero es suficiente, el algoritmo es robusto. De hecho con sensores que dan salida digital también funciona.

==== Modificar el sistema: el actuador ====

Nuestro sistema en este caso será un robot móvil que se mueve con motores de forma autónoma. Y no un diseño cualquiera, sino un diseño con dos motores controlados de forma independiente, uno a cada lado del robot, como sucede en [[:proyectos:sapoconcho|]], [[:proyectos:raptor|]] o [[:proyectos:escornabot|]].

{{:old:archivo:sapoconcho_kis.jpg?400}}

Este diseño -en inglés //differential drive//- permite controlar de manera sencilla el robot con varias acciones:

  * Si las dos ruedas giran hacia delante -o hacia atrás- a la misma velocidad el robot se desplazará en línea recta (más o menos).
  * Si una rueda gira hacia delante y otra hacia atrás, el robot girará sobre si mismo.
  * **Si las dos ruedas giran hacia delante con diferente velocidad, el robot trazará una curva**.
{{:guias:guias:pid:differential-steering-tutorials-42bots2.png?500}}

Imágen adaptada de [[http://42bots.com/tutorials/differential-steering-with-continuous-rotation-servos-and-arduino/|http://42bots.com/tutorials/differential-steering-with-continuous-rotation-servos-and-arduino/]]

Esta última acción es la que nos interesa, hacer que nuestro robot se mueva siguiendo una línea curva. Para eso vamos a fijar una **velocidad base**  para las dos ruedas (mayor o menor para que corra más o menos) y sobre esta vamos a hacer una **corrección**  sumando a una y restando a otra para trazar una curva más o menos cerrada con el siguiente pseudocódigo.

<code>
  velocidad_rueda_izquierda = velocidad_base - corrección
  velocidad_rueda_derecha = velocidad_base + corrección
</code>

{{:guias:guias:pid:velocidad_corregida.png}}

Para pasar esto a un sistema real necesitamos un controlador de motores que podemos usar con las [[:guias:control_de_motores|librerías que ya creamos en Bricolabs]]. En el caso de las librerías de movimiento de Sapoconcho la instrucción tiene esta pinta:

<code>
  sapoconcho.drive(vbase+u,vbase-u,0);
</code>

De este modo ya tenemos los dos elementos: el sensor de líneas, capaz de saber si nos desviamos, y los dos motores, capaces de girar a diferente velocidad para corregir ese desvío.

===== Algoritmos PID: la estrategia de control =====

Conocer el error y tener los medios para corregirlo es la primera parte, ahora necesitamos una estrategia de actuación. Y la desarrollaremos poco a poco.

==== Control proporcional (P) ====

La estrategia más sencilla es corregir en proporción al error. Si la línea se desvía a la derecha, giro a la derecha y al revés. Y lo hago de forma proporcional: cuanto más me desvío más debo girar.

Es decir a mayor error, mayor corrección. Esa proporción se hace estableciendo que la corrección sea igual al error multiplicado por un factor -coeficiente proporcional o kp-

<code>
  p = -4*IR[0] -3*IR[1] -2*IR[2] -IR[3] +IR[4] +2*IR[5] +3*IR[6] +4*IR[7]; // lee el error

  u = kp * p // calcula la corrección

  drive(vbase+u,vbase-u,0); // ejecuta la corrección
</code>

Estas instrucciones deben ahora repetirse de forma indefinida para corregir constantemente el error, pero Arduino es especialista en hacer eso con el loop() ¿no? En Python para Raspberry o cualquier otro sistema podemos hacer un bucle //while(1) o while True//.

Hay una complicación que se nos ha colado sin apenas verla; **ya tenemos dos parámetros que habrá que ajustar**: la velocidad base y el coeficiente kp. Habrá que probar valores por ensayo y error, y no será fácil. Programar un PID es siempre más fácil que ajustarlo o tunearlo. Una velocidad base inicial para probar puede ser alrededor de la mitad de la velocidad máxima del motor, para luego ir subiendo. El valor del coeficiente kp y los próximos que veremos es mejor probar a ajustarlos con saltos grandes -0.001, 0.01, 0.1, 1, 10, 100…- y luego ajustes más finos.

**Advertencia**: según cómo sean los datos de los sensores y los motores y cómo estén situados (a derecha e izquierda) es posible que tengamos el sistema al revés y tengamos que hacer la corrección en sentido contrario. Probaremos por ensayo y error cambiando dónde se suma y dónde se resta la corrección de velocidad.

<code>
  drive(vbase-u,vbase+u,0); // intercambiamos + por -
</code>

==== Control integral (I) ====

En algunas ocasiones el control proporcional no será capaz de hacer una corrección suficiente para llevar el valor de error a cero. En nuestro ejemplo por un una curva que se va cerrando poco a poco. Lo que haremos ahora es tratar de averiguar si ese error se esté acumulando sin ser corregido. La medida de ese error acumulado es el **error integral**, que sale simplemente de ir sumando los errores en cada paso del bucle. Es una medida del error del pasado.

<code>
  p = -4*IR[0] -3*IR[1] -2*IR[2] -IR[3] +IR[4] +2*IR[5] +3*IR[6] +4*IR[7]; // leer el error
  i = i + p; // lo suma al valor integral
</code>

Al principio del programa, este error -la variable i- tendrá que tener un valor inicial cero.

La segunda corrección vendrá de multiplicar el error integral por otro coeficiente -ki- y sumarlo a la corrección.

<code>
  p = ... ...; // lee el error
  i = i + p; // acumula el error
  u = kp * p + ki *i; // calcula la corrección
  drive(vbase+u,vbase-u,0); // ejecuta la corrección
</code>

Y tenemos un nuevo parámetro ki, ya van tres, para tunear el sistema. Para este parámetro probaremos de nuevo valores muy diferentes -0.001, 0.01, 0.1, 1, 10, 100…- y luego ajuste fino.

Una ventaja añadida de usar corrección integral es que si perdemos la línea por fuera del sensor, el sistema recordará que hay un error en ese sentido y que debe ser corregido.

==== Control diferencial (D) ====

Si comparamos el error en cada ciclo del bucle con el anterior podemos ver su diferencia para saber si el error está subiendo o está bajando, y de ese modo tener información sobre el error del futuro. Con esa diferencia -el **error diferencial**- podemos hacer una tercera corrección multiplicando por otro factor, kd. Esta corrección nos ayuda a responder a variaciones muy rápidas del error, como un cambio brusco de dirección en una chicane.

<code>
  p = ... ...; // leer el error

  i=i+p;
  d=p-p_anterior; // diferencia con el último valor
  p_anterior=p; // guarda el valor para el siguiente ciclo del bucle

  u=kp*p+ki*i+kd*d;

  drive(vbase+u,vbase-u,0); // ejecutar la corrección
</code>

Y tenemos un cuarto parámetro, kd, que se ajustará con la misma estrategia de cambios grandes y ajuste fino.

===== La estrategia global y los ajustes =====

El algoritmo PID final **¡tiene sólo seis líneas de código!**, pero aun así es muy poderoso.

<code>
  p = -4*IR[0]-3*IR[1]-2*IR[2]-IR[3]+IR[4]+2*IR[5]+3*IR[6]+4*IR[7];
  i=i+p;
  d=p-p_anterior;
  p_anterior=p;
  u=kp*p+ki*i+kd*d;
  drive(vbase+u,vbase-u,0);
</code>

Aquí puede encontrarse el código completo para un Sapoconcho con 6 sensores: [[https://github.com/felixstdp/sapoconcho/blob/master/examples/pidfollowline/pidfollowline.ino|https://github.com/felixstdp/sapoconcho/blob/master/examples/pidfollowline/pidfollowline.ino]]

Sin embargo, como se dijo más arriba, programarlo es siempre más sencillo que tunearlo ajustando los parámetros. Ojalá pudiese deciros que hay parámetros universales -dependerán del robot y del circuito- o un método universal de ajuste. Pero no es así.

Tampoco son necesarios los tres ajustes siempre, y así tendremos controles P, PI, PD, PID… En sistemas lentos, por ejemplo, el ajuste diferencial no hace falta.

Un posible método de tunning por pasos (que no siempre funciona) es:

  * Probar una velocidad base sobre la mitad de la máxima.
  * Poner ki, kd a cero y ajustar kp subiendo su valor para que no se salga de la línea. Cuando el coche empiece a oscilar a los dos lados bajar un poco kp.
  * Mantener kd a cero y subir ki hasta que empiece a oscilar de nuevo.
  * Ajustar kd.
  * Repetir todos los pasos anteriores para ajuste fino (hasta el infinito y más allá).

Los motores de corriente continua que solemos usar para estos robots varían su velocidad a medida que se gastan la pilas del robot y eso hace muy complicado a veces el ajuste porque los valores con pilas nuevas o gastadas no son los mismos. Eso hace muy recomendable poner algún circuito regulador que garantice que los motores recibe siempre los mismos voltios.

==== Bola extra: corregir el overshooting ====

Los sistemas PID clásicos se montaban con circuitos de electrónica analógica y tenían algunos problemas que no siempre pueden corregirse con el ajuste de parámetros. Uno de ellos es que cuando el error es muy grande, antes de eliminarse puede acumularse mucho error integral y al llegar a cero nos pasamos de vueltas y seguimos corrigiendo en sentido contrario al error real. Esto en inglés se conoce como overshooting.

{{:guias:guias:pid:faq00582-1.jpg?400}}

Pero nuestro sistema no es analógico, es un programa, y si detecta que estamos estamos haciendo una corrección integral de sentido contrario al error, podemos eliminar este valor poniéndolo de nuevo a cero (en inglés integral windup).

<code>
  p = -4*IR[0]-3*IR[1]-2*IR[2]-IR[3]+IR[4]+2*IR[5]+3*IR[6]+4*IR[7];

  i=i+p;
  if ((p*i)<0) i=0;  // corrige el overshooting - integral windup

  d=p-p_anterior;
  p_anterior=p;
  u=kp*p+ki*i+kd*d;
  drive(vbase+u,vbase-u,0);
</code>

O en un código más compacto (y menos legible)

<code>
  i=(i+p)*((i*p)>0);
</code>

==== Segunda bola extra: sólo dos sensores ====

El algoritmo en su versión sencilla con corrección PI puede funcionar con sólo dos sensores. No será el coche más rápido, pero funciona. En serio. Con cuatro líneas de código.

<code>
  p = IR[1]-IR[0]; // lee los sensores y los resta
  i=(i+p)*((i*p)>0);
  u=kp*p+ki*i;
  drive(vbase+u,vbase-u,0);
</code>

==== Tercera bola extra: velocidad variable ====

Este último algoritmo (publicado después de la OSHWDem17) utiliza una velocidad base variable, más rápida en recta, más lenta en curva. Y más parámetros a tunear. La función es no lineal con una velocidad máxima, una mínima y una constante de decaimiento (kv) de una a otra.

<code>
  p = -7*IR[0]-5*IR[1]-3*IR[2]-IR[3]+IR[4]+3*IR[5]+5*IR[6]+7*IR[7];
  i=i+p;
  d=p-p_old;
  p_old=p;
  if ((p*i)<0) i=0;  // integral windup

  u=kp*p+ki*i+kd*d;
  vbase=vmin+(vmax-vmin)*exp(-kv*abs(kp*p));
  drive(vbase+u,vbase-u);
</code>

Lo puedes encontrar entero aquí [[https://github.com/felixstdp/raptor/blob/master/raptor_nl_pid.ino|https://github.com/felixstdp/raptor/blob/master/raptor_nl_pid.ino]]

===== Equipo =====

  * [[:user:felixstdp|Félix]]

===== Enlaces =====

  * [[https://github.com/fstdp/raptor/blob/master/raptor_qtr8rc.ino|Siguelíneas PID con QTR-8RC]]
  * [[https://github.com/fstdp/sapoconcho/blob/master/examples/pidfollowline/pidfollowline.ino|Siguelíneas PID con QTR-8A]]