Control de velocidad de ruedas en bucle cerrado con encoders

El PWM es un voltaje, no una velocidad

analogWrite(motorPin, 180) no pide una velocidad. Pide alrededor del 70 % del voltaje de la batería, y qué velocidad produce eso depende de la carga sobre la rueda, del rozamiento de esa reductora, del estado de carga y de la temperatura de las scopetas.

Por eso un robot con dos motores idénticos y el mismo PWM se va en curva. Nunca fueron idénticos — una reductora está más apretada, un juego de scopetas es más nuevo, y al mismo voltaje se diferencian entre un 3 y un 8 %. A lo largo de tres metros eso es una curva visible.

En un robot de tracción diferencial puedes tapar eso con una constante de trim. En una base mecanum no puedes, porque la cinemática inversa reparte entre las cuatro ruedas objetivos salvajemente distintos — en el ejemplo desarrollado de ahí, una rueda recibe 0,020 m/s y otra 0,580 m/s de la misma orden— y esos ratios son lo que decide la dirección que toma el robot. Una rueda que va un 5 % lenta no hace el robot ligeramente lento; lo hace ir a otro sitio.

Cerrar el lazo sobre la velocidad medida de cada rueda es lo que arregla eso, y es el mismo arreglo para cualquier robot con más de dos ruedas o con cualquier necesidad de llegar a algún sitio.

Mide la velocidad que realmente tienes

Empieza por los conteos de encoder en cuadratura. La conversión es geometría fija:

counts per wheel revolution = CPR_motor × gear ratio × 4     (×4 for quadrature edges)
metres per count            = 2πr / counts per revolution

Para un encoder de motor de 12 CPR detrás de una reductora 30:1 sobre ruedas de 97 mm: 12 × 30 × 4 = 1440 cuentas/rev, y 2π × 0,0485 / 1440 = 212 µm por cuenta.

Después la parte que todo el mundo hace mal:

// DON'T: counts since last loop, divided by loop time.
float speed = (count - lastCount) * METRES_PER_COUNT / dt;

A 100 Hz y 0,05 m/s eso es una cuenta por muestra. Tu señal de velocidad es 0, 212, 0, 212 µm — una onda cuadrada con un rizado de ±100 %, y un término D la amplificará hasta un motor que zumba. Tres salidas, y normalmente quieres la primera:

1. Muestrea más lento de lo que controlas. Calcula la velocidad sobre 20 ms (50 Hz) y ejecuta el bucle a 50 Hz. Más cuentas por ventana, menos cuantización, y sin retardo de filtro.
2. Mide el tiempo entre flancos en vez de los flancos en un tiempo. Excelente a baja velocidad, incómodo a alta velocidad, y necesita marcas de tiempo de interrupción.
3. Pasa la velocidad por un paso bajo. Lo más barato, y te cuesta margen de fase — que es lo que menos tienes.

// A 50 Hz velocity estimate, with the counter read atomically because an
// encoder ISR is writing it.
noInterrupts();
long count = encoderCount;
interrupts();

float dt = (now - lastTime) * 1e-6f;             // micros(), so no ms rounding
float v = (count - lastCount) * METRES_PER_COUNT / dt;
lastCount = count; lastTime = now;

volatile long encoderCount y el corchete noInterrupts() no son opcionales: una lectura de 32 bits son varias instrucciones en un AVR, y una ISR que caiga en mitad te da un número que nunca fue cierto.

Primero prealimentación, después PID

El instinto es ir a por el PID. Haz primero el 90 % fácil.

La velocidad en régimen permanente de un motor DC es casi lineal con el voltaje aplicado, así que si conoces la velocidad que quieres puedes calcular la mayor parte del PWM:

pwm ≈ kS·sign(v) + kV·v

• kV — la pendiente. Cuánto PWM compra un m/s.
• kS — la ordenada en el origen. El PWM necesario solo para romper el rozamiento estático, antes de que la rueda se mueva.

Mide ambos en diez minutos:

1. Pon el robot sobre calzos para que las ruedas giren libres.
2. Sube el PWM despacio hasta que la rueda empiece justo a girar. Eso es kS, típicamente 25–45 sobre 255.
3. Pon el PWM en 100, 150, 200, 250 y anota la velocidad en régimen en cada uno.
4. Ajusta una recta. Su pendiente es kV, y debería extrapolar de vuelta aproximadamente a tu kS.

Ahora el lazo solo tiene que corregir el error entre esa estimación y la realidad — que es pequeño, así que las ganancias pueden ser pequeñas, así que todo el asunto queda estable y silencioso:

float feedforward = kS * sign(target) + kV * target;
float output = feedforward + kP * error + kI * integral;

Sin prealimentación, el integrador tiene que construir la orden entera desde cero cada vez que el objetivo cambia. Eso es lento, se pasa de rosca, y es la razón de que tantos bucles de motor ajustados a mano acaben con un kI enorme y un robot que se lanza.

El controlador

P e I. Nada de D.

struct WheelLoop {
  float kS, kV, kP, kI;
  float integral = 0, target = 0;

  int update(float measured, float dt) {
    float error = target - measured;
    float ff = kS * (target > 0 ? 1 : target < 0 ? -1 : 0) + kV * target;
    float unclamped = ff + kP * error + kI * (integral + error * dt);
    int   output    = (int)constrain(unclamped, -255, 255);

    // Conditional integration: only accumulate if the output is NOT saturated,
    // or if the error would wind the integrator back toward the middle.
    // Otherwise a stalled wheel builds a command it cannot use and then keeps
    // driving for a second after you let go of the stick.
    if (fabsf(unclamped) < 255.0f || (error * unclamped) < 0) integral += error * dt;

    return output;
  }
};

Por qué no hay derivativo: el término D deriva la señal de velocidad, que ya es una diferencia de cuentas de encoder. Estás derivando dos veces, lo que significa amplificar dos veces el ruido de cuantización, y en una rueda casi no hay nada que amortiguar en ningún caso. Un lazo de velocidad con PI y buena prealimentación es la respuesta estándar, y añadir D es una de las formas más habituales de empeorar un lazo que funcionaba. (Un lazo de posición es otra cosa — mira el control en cascada.)

El anti-windup es la parte que no puedes saltarte. Sujeta el robot contra una pared con una orden de velocidad aplicada, y un integrador ingenuo rampa a fondo en un par de segundos. Suelta y el robot sale disparado. La integración condicional, un if, lo arregla.

Ajuste, en el orden correcto

1. Solo prealimentación. kP = kI = 0. Ordena 0,3 m/s y mide lo que obtienes. Dentro de un 10 % aproximadamente significa que kV es correcta. Arréglala antes de tocar nada más.
2. Sube kP hasta que zumbe, y luego retrocede un tercio. Estás escuchando un quejido que aparece con la ganancia y desaparece sin ella.
3. kI hasta que el error en régimen desaparezca. Una rueda con la prealimentación correcta necesita muy poco. Empieza en kP/10 por segundo y ve subiendo poco a poco.
4. Compruébalo con carga, no en calzos. Todo el punto es la rueda a la que se le pide más.
5. Prueba las cuatro juntas, conduciendo en diagonal. Esa es la orden con la mayor dispersión de objetivos de rueda, y es donde primero se nota una rueda mal ajustada.

El simulador de PID es una forma más rápida de construir la intuición para los pasos 2 y 3 que un robot, y la guía de ajuste tiene el procedimiento general.

Tasa de muestreo

La tasa del bucle tiene que estar muy por encima de lo que controla. La velocidad de un motor DC pequeño con reductora se asienta con una constante de tiempo de unos 50–150 ms; diez muestras dentro de esa constante es la regla habitual, así que 50–200 Hz.

Ejecútalo con un calendario fijo, no «tan rápido como vaya loop()», porque dt aparece directamente tanto en la estimación de velocidad como en el integrador:

const unsigned long PERIOD_US = 10000;   // 100 Hz
static unsigned long next = 0;

void loop() {
  unsigned long now = micros();
  if ((long)(now - next) < 0) return;    // signed compare survives rollover
  next += PERIOD_US;
  // ... read encoders, run four loops, write four PWMs
}

La comparación con signo es la misma trampa del desbordamiento de millis() en su forma de micros(), donde muerde a los 71 minutos en vez de a los 49 días.

Lo que te compra, medido

Mismo robot, misma orden, mismo suelo:

Esa última fila es la menos apreciada. Un lazo cerrado sabe cuándo ha fallado: una rueda cuya salida está clavada en 255 y sigue por debajo del objetivo está informando de algo útil, y puedes poner eso en una línea de telemetría, rechazar la orden, o frenar el robot entero para mantener honestos los ratios.

Resolución de problemas

Siguiente

Las ruedas ya dan las velocidades que se les piden. Orienta todo el conjunto hacia donde el conductor quiere con orientación en campo, y dale objetivos suaves en vez de escalones con perfiles de movimiento. Después mira de qué te puede salvar y de qué no un lazo de velocidad en el simulador de locomoción mecanum — una velocidad de rueda perfectamente mantenida sigue sin significar nada si el suelo deja de agarrar.