Controllo PID in cascata per un robot autobilanciato

Introduzione

Hai un angolo di inclinazione affidabile. Ora devi mantenere il robot in piedi e poi, alla fine, farlo restare in un posto invece di scivolare in giro per la stanza pur restando tecnicamente in verticale.

Sono due compiti diversi e l’errore che quasi tutti commettono al primo tentativo è cercare di svolgerli con un solo anello. Questo tutorial li costruisce come due anelli annidati, il modo in cui vengono controllati i veri robot autobilanciati.

Perché un solo anello non basta

Ciò che vuoi è un robot che resti dove lo hai messo. Ciò che puoi comandare è la coppia motrice. Tra i due c’è un fatto scomodo: l’unico modo per cambiare la posizione del robot è cambiarne prima l’inclinazione.

Prova a controllare direttamente la posizione, azionando i motori in proporzione all’errore di posizione: le ruote si sposteranno da sotto il baricentro. Il corpo ruoterà nel verso opposto e il robot cadrà immediatamente, muovendosi nella direzione sbagliata mentre cade. Non è un problema di regolazione: il segno della risposta iniziale è genuinamente inverso, una proprietà chiamata a fase non minima.

La soluzione consiste nel controllare le due grandezze a velocità diverse, in cascata.

La struttura

   position error → [ OUTER PID ] → tilt target
                                        ↓
   tilt error     → [ INNER PID ] → motor command → robot
                                        ↑
                                   measured tilt

L’anello interno svolge un solo compito: mantenere l’angolo di inclinazione che gli viene indicato. Funziona rapidamente, a 200 Hz o più, perché deve contrastare la gravità, che non aspetta.

L’anello esterno svolge l’altro compito: portare il robot in una posizione. Non può agire direttamente sui motori, quindi chiede all’anello interno una piccola inclinazione. Quando si trova dietro la posizione desiderata, richiede di inclinarsi in avanti; l’anello interno aziona le ruote per mantenere quell’inclinazione; mantenere un’inclinazione in avanti significa accelerare continuamente in avanti. Funziona forse dieci volte più lentamente dell’anello interno.

Questa separazione tra le frequenze costituisce l’intero progetto. Se l’anello esterno funziona a una frequenza vicina a quella dell’anello interno, i due si contraddicono: l’anello esterno modifica l’obiettivo più rapidamente di quanto l’anello interno riesca a seguirlo e il robot oscilla finché non collide con il pavimento.

L’anello interno

float angleError = angle - tiltTarget;      // measured MINUS target
angleIntegral   = constrain(angleIntegral + angleError * dt, -3, 3);

float command = Kp * angleError
              + Ki * angleIntegral
              + Kd * gyroRate;

command = constrain(command, -255, 255);

Tre dettagli in questo codice contano più dei guadagni.

L’errore è il valore misurato meno quello desiderato, l’opposto del setpoint − measurement dei libri di testo. Per intercettare un robot che cade in avanti aziona le ruote in avanti, sotto la massa in caduta, proprio come infili la mano sotto una scopa. Quindi un’inclinazione positiva deve produrre un comando motore positivo. Inverti questo segno e il controllore spingerà il robot contro il pavimento appena lo attivi: un guasto caratteristico e violento, quasi sempre dovuto proprio al segno.

Il termine derivativo usa direttamente la velocità angolare del giroscopio, non la differenza tra angoli successivi. Il giroscopio misura la velocità angolare come suo segnale nativo. Differenziare un segnale angolare rumoroso amplifica esattamente il rumore che vorresti amplificare meno ed è il motivo per cui molti primi tentativi richiedono un guadagno D insolitamente basso per restare stabili.

L’integrale è limitato. Durante una caduta i motori saturano e l’errore continua a crescere, quindi un integrale non limitato cresce fino a un valore enorme che poi ostacola il recupero durante il ritorno. Limitane il valore e smetti del tutto di accumulare quando l’uscita è satura.

L’anello esterno

if (++slowTick >= 10) {                     // ten times slower
  slowTick = 0;
  float positionError = targetPosition - wheelPosition;
  tiltTarget = constrain(KpPos * positionError - KdPos * wheelVelocity,
                         -0.12, 0.12);      // radians, about ±7°
}

wheelPosition deriva dalla somma degli incrementi del tuo encoder. Se non hai encoder, puoi integrare il comando motore come approssimazione grossolana, ma l’anello sarà notevolmente peggiore.

Il limite non è facoltativo. Un anello esterno che può richiedere 30° assegna all’anello interno un obiettivo che non riesce fisicamente a mantenere: il robot si impegna in un’inclinazione da cui non può recuperarsi e cade mentre obbedisce perfettamente agli ordini. Bastano pochi gradi; un’inclinazione di 7° accelera già notevolmente un robot.

Nota che il termine di velocità viene sottratto. Attenua l’avvicinamento, così il robot non oltrepassa il bersaglio e non deve tornare indietro.

Regolazione, nell’ordine che funziona

Regola prima soltanto l’anello interno. Imposta KpPos e KdPos a zero, così l’anello esterno rimane inattivo e tiltTarget resta a zero. Il robot resterà in equilibrio sul posto e scivolerà lentamente: è previsto ed è accettabile per ora.

1. Imposta a zero Ki e Kd. Aumenta Kp finché il robot non reagisce in modo brusco a una spinta. Se è troppo basso, cede verso il pavimento senza opporsi davvero; se è troppo alto, ronzia.
2. Aumenta Kd finché l’oscillazione non si smorza. È il termine che trasforma un robot che vibra finché non cade in uno che si stabilizza. Nel simulatore di equilibrio puoi impostare D a zero mentre il robot è in funzione e contare le oscillazioni prima che cada.
3. Aumenta Ki per ultimo e con cautela. Elimina la lenta inclinazione residua causata dalla regolazione del montaggio dell’IMU. Se è troppo grande, l’integrale supera il valore corretto e si scarica, producendo un lento dondolio che si amplifica.

Solo allora abilita l’anello esterno e imposta KpPos intorno a un decimo di un valore che ti sembra ragionevole. Il suo effetto è indiretto: devi osservare se il robot smette di scivolare, non cercare una risposta immediata.

Modalità di guasto e relativo significato

Disattiva i motori oltre il limite di recupero

if (fabs(angle) > FALL_ANGLE) {             // about 35 degrees
  driveMotors(0, 0);
  angleIntegral = 0;
  return;
}

Oltre circa 35°, l’accelerazione orizzontale necessaria per riportare il robot sotto il baricentro supera ciò che le ruote possono fornire prima che il corpo raggiunga il pavimento. Il controllore non lo sa e continuerà a faticare alla massima coppia motrice per tutta la caduta, sollecitando duramente i riduttori e rendendo il guasto più rumoroso del necessario.

La disattivazione azzera anche l’integrale, così quando rimetti in piedi il robot l’anello riparte pulito anziché conservare una correzione accumulata durante la caduta.

Come proseguire

I guadagni qui interagiscono più fortemente che in un PID ad anello singolo, perché il sistema è instabile: non esiste un valore «all’incirca corretto» che si limiti a comportarsi male. Il robot resta in piedi oppure cade.

Questo rende un simulatore particolarmente utile: puoi impostare D a zero su un robot attualmente in equilibrio, osservare il guasto esatto e annullarlo senza riparare nulla. Prova le impostazioni nel simulatore di equilibrio, poi costruisci il progetto reale nel progetto del robot autobilanciato.