Controllo a ciclo chiuso della velocità delle ruote con encoder

Il PWM è una tensione, non una velocità

analogWrite(motorPin, 180) non chiede una velocità. Chiede circa il 70% della tensione della batteria, e la velocità che ne deriva dipende dal carico sulla ruota, dall’attrito nel riduttore, dallo stato di carica e dalla temperatura delle spazzole.

Ecco perché un robot con due motori identici e PWM identici percorre un arco. Non erano mai identici: un riduttore è più stretto, un gruppo di spazzole è più nuovo e alla stessa tensione i due motori differiscono del 3–8%. Su tre metri, ciò produce una curva ben visibile.

Su un robot a trazione differenziale puoi mascherare il problema con una costante di regolazione. Su una base mecanum non puoi, perché la cinematica inversa assegna alle quattro ruote obiettivi molto diversi: nell’esempio riportato, una ruota riceve 0.020 m/s e un’altra 0.580 m/s dallo stesso comando. Sono quei rapporti a determinare la direzione del robot. Una ruota il 5% più lenta non rende il robot leggermente più lento; lo fa andare altrove.

Chiudere il ciclo sulla velocità misurata di ogni ruota risolve il problema, ed è la stessa correzione per ogni robot con più di due ruote o con la necessità di arrivare da qualche parte.

Misura la velocità che hai davvero

Parti dagli incrementi degli encoder in quadratura. La conversione è geometria fissa:

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

Per un encoder di un motore da 12 CPR dietro un riduttore 30:1 e ruote da 97 mm: 12 × 30 × 4 = 1440 counts/rev, mentre 2π × 0.0485 / 1440 = 212 µm per count.

Poi arriva la parte che quasi tutti sbagliano:

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

A 100 Hz e 0.05 m/s si ha un conteggio per campione. Il segnale di velocità è 0, 212, 0, 212 µm, un’onda quadrata con ondulazione del ±100%; il termine D lo amplificherebbe fino a far ronziare il motore. Ci sono tre soluzioni e in genere vuoi la prima:

1. Campiona più lentamente di quanto controlli. Calcola la velocità su 20 ms, a 50 Hz, ed esegui il ciclo a 50 Hz. Hai più conteggi per finestra, meno quantizzazione e nessun ritardo di filtraggio.
2. Misura il tempo tra i fronti anziché il numero di fronti in un intervallo di tempo. È eccellente a bassa velocità, scomodo ad alta velocità e richiede marcatori temporali degli interrupt.
3. Applica un filtro passa-basso alla velocità. È l’opzione più economica, ma costa margine di fase, proprio la risorsa che scarseggia.

// 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 e il blocco noInterrupts() non sono facoltativi: su un AVR la lettura a 32 bit richiede diverse istruzioni e un ISR che vi entra nel mezzo fornisce un valore che non è mai stato vero.

Prima il feed-forward, poi il PID

L’istinto è ricorrere al PID. Fai prima il 90% più semplice.

La velocità a regime di un motore CC è quasi lineare rispetto alla tensione applicata, quindi se conosci la velocità desiderata puoi calcolare gran parte del PWM:

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

• kV — la pendenza. Quanto PWM serve per ottenere 1 m/s.
• kS — l’intercetta. Il PWM necessario soltanto per superare l’attrito statico, prima che la ruota si muova.

Misura entrambi in dieci minuti:

1. Metti il robot su blocchi, così le ruote girano liberamente.
2. Aumenta lentamente il PWM finché la ruota inizia appena a girare. Questo è kS, in genere da 25 a 45 su 255.
3. Imposta il PWM a 100, 150, 200, 250 e registra la velocità a regime a ogni valore.
4. Fai passare una retta. La pendenza è kV e l’estrapolazione dovrebbe riportare circa al tuo kS.

Ora il ciclo deve soltanto correggere l’errore fra quella stima e la realtà. L’errore è piccolo, quindi anche i guadagni possono esserlo e l’intero sistema risulta stabile e silenzioso:

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

Senza feed-forward, l’integratore deve costruire l’intero comando da zero ogni volta che il riferimento cambia. È lento, causa overshoot ed è il motivo per cui tanti cicli di controllo motori regolati a mano finiscono con un kI enorme e un robot che scatta in avanti.

Il controllore

P e I. Non 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;
  }
};

Perché non usare la derivata: il termine D differenzia il segnale di velocità, che è già una differenza di conteggi dell’encoder. Differenzieresti due volte, amplificando due volte il rumore di quantizzazione, e su una ruota non c’è quasi nulla da smorzare. Un ciclo di velocità con PI e buon feed-forward è la risposta standard; aggiungere D è uno dei modi più comuni di peggiorare un ciclo funzionante. Un ciclo di posizione è diverso: vedi il controllo in cascata.

L’antiwindup è la parte che non puoi saltare. Tieni il robot contro un muro mentre applichi un comando di velocità e un integratore naïve raggiunge il fondo scala in un paio di secondi. Lascia andare e il robot parte a tutta velocità. L’integrazione condizionale, un solo if, risolve il problema.

Regolazione, nell’ordine giusto

1. Solo feed-forward. kP = kI = 0. Comanda 0.3 m/s e misura ciò che ottieni. Una differenza entro circa il 10% significa che kV è corretto. Correggilo prima di toccare qualsiasi altro elemento.
2. Aumenta kP finché ronzia, poi riducilo di un terzo. Devi ascoltare un fischio che compare con il guadagno e scompare senza di esso.
3. Aumenta kI finché l’errore a regime scompare. Una ruota con un feed-forward corretto ne richiede pochissimo. Parti da kP/10 al secondo e aumenta lentamente.
4. Provalo sotto carico, non sui blocchi. L’intero punto è la ruota alla quale viene richiesto di più.
5. Prova tutte e quattro insieme, mentre guidi in diagonale. È il comando con la maggiore dispersione tra gli obiettivi delle ruote ed è il primo punto in cui emerge una ruota regolata male.

Il simulatore PID è un modo più rapido del robot per costruire l’intuizione per i passaggi 2 e 3, mentre la guida alla regolazione presenta la procedura generale.

Frequenza di campionamento

La frequenza del ciclo deve essere molto superiore a quella della grandezza controllata. La velocità di un piccolo motore CC con riduttore si assesta con una costante di tempo di circa 50–150 ms; la regola abituale prevede dieci campioni in quella costante, quindi 50–200 Hz.

Esegui il ciclo secondo una pianificazione fissa, non «veloce quanto riesce a eseguire loop()», perché dt compare direttamente sia nella stima della velocità sia nell’integratore:

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
}

Il confronto con segno è la stessa trappola dell’overflow del contatore in millis() nella sua forma micros(), dove si manifesta dopo 71 minuti anziché 49 giorni.

Che cosa ti compra, misurato

Stesso robot, stesso comando, stesso pavimento:

L’ultima riga è sottovalutata. Un controllo a ciclo chiuso sa quando è fallito: una ruota la cui uscita è bloccata a 255 e resta comunque sotto il riferimento sta segnalando qualcosa di utile. Puoi riportarlo su una linea di telemetria, rifiutare il comando o rallentare tutto il robot per mantenere corretti i rapporti.

Risoluzione dei problemi

Prossimi passi

Ora le ruote erogano le velocità richieste. Orienta l’intero sistema nella direzione desiderata dal pilota con l’azionamento orientato al campo e fornisci obiettivi fluidi anziché gradini mediante profili di movimento. Poi osserva che cosa un ciclo di velocità può e non può salvarti dal simulatore della trazione mecanum: una velocità della ruota perfettamente mantenuta resta priva di significato se il pavimento cede.