Simulatore robot bilanciatore: PID per pendolo invertito

Controlli

Avvia, Pausa e Reimposta eseguono l’anello. Spingi applica un impulso come un dito, mentre Anello on/off disattiva entrambi i controllori per vedere cadere il solo processo.

Il controllo più diretto è il robot stesso: trascina il corpo in qualsiasi angolo e lascialo andare. La posizione in cui lo lasci è quella da cui parte il controllore — non c’è una fase di riattivazione e i cursori dei guadagni si applicano immediatamente alla simulazione in esecuzione. Porta il guadagno derivativo a zero mentre il robot è in equilibrio e inizia a oscillare nello stesso istante.

Il menu Taratura carica quattro preset: uno funzionante e tre che falliscono nei tre modi in cui falliscono i bilanciatori reali.

Porta il focus sul canvas e usa Space, S, L, R e F per gli stessi controlli dalla tastiera.

Teoria

Tenere un robot in piedi su due ruote significa mantenere in verticale un pendolo invertito. Il baricentro si trova sopra l’asse, quindi la verticale è un equilibrio nello stesso senso in cui lo è una matita appoggiata sulla punta: matematicamente reale, fisicamente impossibile senza correzioni costanti. Inclinalo di qualsiasi angolo e la gravità produce una coppia che lo inclina ulteriormente. Più va lontano, più rapidamente va.

Il robot ha esattamente un modo per contrastare quella coppia: può spostare l’asse. Accelerando le ruote in avanti spinge la base di nuovo sotto il baricentro, facendo ruotare il corpo verso la verticale. Ecco perché la correzione sembra invertita all’inizio: per fermare una caduta in avanti devi avanzare, proprio come sposti la mano sotto una scopa che stai bilanciando invece di allontanarla.

Questo dà al robot un limite rigido. Oltre circa 35°, l’accelerazione orizzontale necessaria supera ciò che le ruote possono fornire prima che il corpo colpisca il pavimento, e nessun aumento del guadagno aiuta. Le linee tratteggiate sul tracciato indicano quel confine.

Algoritmo

Il controllore è in cascata — due anelli PID, uno alimenta l’altro.

L’anello interno funziona rapidamente e mantiene un angolo di inclinazione. Confronta l’angolo misurato con un obiettivo e comanda la coppia motore:

error   = measured_angle − target_angle
motor   = Kp·error + Ki·∫error + Kd·d(measured)/dt

Nota che l’errore è misurato meno l’obiettivo, non l’obiettivo meno il valore misurato secondo il libro. Fermare una caduta in avanti significa far avanzare le ruote, quindi un’inclinazione positiva deve produrre un comando motorio positivo. Questo segno è il motivo più comune per cui il primo robot bilanciatore si schianta a terra subito dopo l’accensione.

L’anello esterno funziona più lentamente e mantiene una posizione. Non può comandare i motori — sono già occupati — quindi comanda invece un obiettivo di inclinazione per l’anello interno:

target_angle = Kp_pos·(desired_position − position) − Kd_pos·velocity

Quando si trova dietro il punto in cui vuole essere, chiede una piccola inclinazione in avanti; l’anello interno poi fa avanzare le ruote per mantenere quell’inclinazione e il robot avanza. L’inclinazione viene deliberatamente limitata a pochi gradi. Un anello esterno che può richiedere un angolo elevato assegnerà all’anello interno un obiettivo che non può mantenere, e perderanno entrambi nello stesso momento.

Il termine integrale conta più qui che nella maggior parte degli anelli, perché assorbe la differenza tra dove l’IMU ritiene che sia la verticale e dove si trova realmente. È anche il motivo per cui l’integrale deve smettere di accumulare mentre i motori sono saturati — altrimenti si carica durante una caduta che non ha la potenza per prevenire e poi ostacola il recupero.

Codice sorgente

// Inner loop: tilt → motors. Runs every 5 ms off a timer, not in loop().
float angleError = angle - tiltTarget;          // measured minus target
angleIntegral   = constrain(angleIntegral + angleError * dt, -3, 3);
float command   = Kp * angleError
                + Ki * angleIntegral
                + Kd * gyroRate;                // rate straight from the gyro

command = constrain(command, -255, 255);
if (fabs(angle) > FALL_ANGLE) command = 0;      // past saving; cut the motors
driveMotors(command, command);

// Outer loop: position → tilt target. Runs ten times slower, and may only
// ever ask for a small lean.
if (++slowTick >= 10) {
  slowTick = 0;
  float positionError = 0 - wheelPosition;
  tiltTarget = constrain(KpPos * positionError - KdPos * wheelVelocity,
                         -0.12, 0.12);          // radians, about ±7°
}

Due dettagli passano direttamente da questo simulatore. Prendi il termine derivativo dalla velocità angolare del giroscopio invece di differenziare l’angolo: il giroscopio misura direttamente quella velocità e senza il rumore che la differenziazione amplifica. Inoltre, interrompi i motori oltre l’angolo di recupero invece di lasciare che il controllore continui a fare forza — risparmia i riduttori e rende il fallimento evidente invece che violento.

Hardware

Il sensore è un MPU6050, che contiene sia un accelerometro sia un giroscopio, perché nessuno dei due da solo è utilizzabile. L’accelerometro sa dove si trova il basso, ma viene saturato dal movimento del robot stesso; il giroscopio segue bene la rotazione, ma deriva in pochi secondi. Un filtro complementare li fusiona in un angolo stabile e rapido.

I motori devono avere un rapporto di riduzione abbastanza basso da fornire coppia e abbastanza rapido da recuperare una caduta — i motori con riduttore N20 intorno a 200 RPM sono una scelta comune, azionati da un TB6612FNG, la cui bassa caduta di tensione conta quando l’anello richiede piccole correzioni migliaia di volte al secondo.

Costruiscilo davvero nel progetto del robot autobilanciante oppure inizia dal tutorial sulla lettura dell’inclinazione se l’IMU è nuovo per te.

Ipotesi e differenze hardware

Il modello è un pendolo invertito 2D con un solo asse di inclinazione, collegamenti rigidi, nessuno slittamento delle ruote e un IMU che segnala l’angolo e la velocità reali con solo la deriva e il rumore che imposti. Un robot bilanciatore reale sente una coppia motore che varia con la tensione della batteria, la quantizzazione degli encoder, l’offset di montaggio dell’IMU e la deriva termica; inoltre può cadere di lato — un grado di libertà che questo laboratorio elimina completamente. La struttura PID in cascata e il derivativo basato sulla velocità del giroscopio mostrate qui si trasferiscono direttamente, ma la taratura del montaggio, quella del filtro e il limite dell’angolo di caduta devono essere ritrovati sul telaio fisico.

Schema elettrico

Due regole di cablaggio decidono se questo robot funziona.

La batteria dei motori raggiunge il pin VM del driver e nient’altro. La corrente dei motori non deve mai attraversare l’Arduino e ogni massa — scheda, driver, IMU, polo negativo della batteria — deve essere collegata insieme, altrimenti i segnali di controllo non hanno un riferimento comune e il fallimento è silenzioso anziché evidente.

L’encoder è alimentato dalla logica, non dalla pista dei motori. Alimentare un encoder con l’alimentazione dei motori lo distrugge o riempie i conteggi di rumore di commutazione. Collega anche il canale A dell’encoder a un pin in grado di generare interrupt; leggerlo a polling da loop() perde conteggi, e i conteggi persi fanno perdere lentamente all’anello esterno il riferimento alla posizione del robot.

Monta l’IMU vicino all’asse. Montarlo in alto moltiplica l’accelerazione lineare che vede durante ogni correzione, rendendo più difficile il problema della fusione senza alcun vantaggio.