Arduino Robot autobilanciante

Ciao a tutti!

In questo istruttivo, ti mostrerò come costruire un piccolo robot auto-bilanciante che può muoversi evitando ostacoli. Si tratta di un piccolo robot di 4 pollici di larghezza e 4 di altezza, basato sulla scheda di sviluppo Arduino Pro Mini e sul modulo accelerometro-giroscopio MPU6050.

Nei passaggi che seguono, vedremo come interfacciare l'MPU6050 con Arduino, come misurare l'angolo di inclinazione del robot, come utilizzare il PID per mantenere il robot in equilibrio. Un robot a ultrasuoni viene inoltre aggiunto al robot che gli impedisce di sbattere contro gli ostacoli mentre si aggira.

Elenco delle parti

Ho comprato la maggior parte di questi pezzi da aliexpress ma li puoi trovare anche in qualsiasi altro negozio di elettronica.

1. Arduino Pro Mini

2. Modulo GY-521 con MPU-6050

3. Driver motore Pololu DRV8833

4. Convertitore boost 2, 5V

5. Sensore di distanza ultrasonico US-020

6. Batteria e supporto NCR18650

7. Coppia di motoriduttori in metallo micro (N20, 6 V, 200 giri / min) e staffe

8. Coppia di ruote 42x19mm

9. 3, PCB prototipo a doppia faccia (4 cm x 6 cm)

10. 8, distanziali in nylon da 25 cm e 4 dadi in nylon

Oltre a quanto sopra, avrai bisogno di alcuni cavi, connettori berg e un interruttore on / off.

Step 1: Un po 'di teoria

Cominciamo con alcuni concetti fondamentali prima di sporcarci le mani.

Il robot autobilanciante è simile a un pendolo capovolto. A differenza di un normale pendolo che continua a oscillare una volta dato una spinta, questo pendolo invertito non può rimanere in equilibrio da solo. Cadrà semplicemente. Allora come lo bilanciamo? Considera di bilanciare un manico di scopa sul nostro indice, che è un classico esempio di bilanciamento di un pendolo invertito. Muoviamo il dito nella direzione in cui cade il bastone. Simile è il caso di un robot auto-bilanciante, solo che il robot cadrà in avanti o indietro. Proprio come nel bilanciare un bastone con un dito, bilanciamo il robot guidando le ruote nella direzione in cui sta cadendo. Quello che stiamo cercando di fare qui è di mantenere il baricentro del robot esattamente sopra il punto di articolazione.

Per guidare i motori abbiamo bisogno di alcune informazioni sullo stato del robot. Dobbiamo conoscere la direzione in cui il robot sta cadendo, la sua inclinazione e la velocità con cui sta cadendo. Tutte queste informazioni possono essere dedotte dalle letture ottenute da MPU6050. Combiniamo tutti questi ingressi e generiamo un segnale che guida i motori e mantiene equilibrato il robot.

Step 2: Iniziamo a costruire

Per prima cosa completeremo i circuiti e la struttura del robot. Il robot è costruito su tre strati di pannelli di perforazione distanziati di 25 mm mediante distanziali in nylon. Lo strato inferiore contiene i due motori e il driver del motore. Lo strato intermedio ha i moduli controller, IMU e regolatore di boost 5V. Lo strato più in alto ha la batteria, un interruttore on / off e il sensore di distanza ad ultrasuoni (lo installeremo verso la fine una volta che il robot sarà bilanciato).

Prima di iniziare a realizzare il prototipo su un pannello perforato, dovremmo avere un quadro chiaro di dove posizionare ogni parte. Per semplificare la prototipazione, è sempre meglio disegnare il layout fisico di tutti i componenti e utilizzarlo come riferimento per posizionare i componenti e instradare i ponticelli sul pannello di perforazione. Una volta che tutte le parti sono state posizionate e saldate, collegare le tre schede utilizzando distanziali in nylon.

Potresti aver notato che ho usato due moduli regolatori di tensione separati per pilotare i motori e il controller anche se entrambi richiedono una sorgente 5V. Questo è molto importante. Nel mio primo progetto, ho usato un singolo regolatore boost da 5 V per alimentare il controller e i motori. Quando ho acceso il robot, il programma si blocca in modo intermittente. Ciò era dovuto al rumore generato dal circuito del motore che agisce sul controller e sull'IMU. Ciò è stato effettivamente eliminato separando il regolatore di tensione dal controller e dal motore e aggiungendo un condensatore da 10uF ai terminali di alimentazione del motore.

Passaggio 3: misurazione dell'angolo di inclinazione mediante l'accelerometro

MPU6050 ha un accelerometro a 3 assi e un giroscopio a 3 assi. L'accelerometro misura l'accelerazione lungo i tre assi e il giroscopio misura la frequenza angolare attorno ai tre assi. Per misurare l'angolo di inclinazione del robot abbiamo bisogno di valori di accelerazione lungo gli assi ye z. La funzione atan2 (y, z) fornisce l'angolo in radianti tra l'asse z positivo di un piano e il punto dato dalle coordinate (z, y) su quel piano, con segno positivo per gli angoli in senso antiorario (metà destra piano, y> 0) e segno negativo per gli angoli in senso orario (semipiano sinistro, y <0). Usiamo questa libreria scritta da Jeff Rowberg per leggere i dati da MPU6050. Carica il codice indicato di seguito e osserva come varia l'angolo di inclinazione.

#include "Wire.h"
#include "I2Cdev.h" #include "MPU6050.h" #include "math.h"

MPU6050 mpu;

int16_t accY, accZ; float accAngle;

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

void loop () {accZ = mpu.getAccelerationZ (); accY = mpu.getAccelerationY (); accAngle = atan2 (accY, accZ) * RAD_TO_DEG; Se (isNaN (accAngle)); else Serial.println (accAngle); }

Prova a muovere il robot in avanti e all'indietro mantenendolo inclinato ad un angolo fisso. Noterai che l'angolo mostrato nel tuo monitor seriale cambia improvvisamente. Ciò è dovuto alla componente orizzontale dell'accelerazione che interferisce con i valori di accelerazione degli assi ye z.

Passaggio 4: misurazione dell'angolo di inclinazione mediante giroscopio

Il giroscopio a 3 assi di MPU6050 misura la velocità angolare (velocità di rotazione) lungo i tre assi. Per il nostro robot autobilanciante, la velocità angolare lungo il solo asse x è sufficiente per misurare la velocità di caduta del robot.

Nel codice indicato di seguito, leggiamo il valore del giroscopio attorno all'asse x, lo convertiamo in gradi al secondo e quindi lo moltiplichiamo per il tempo di ciclo per ottenere il cambio di angolo. Aggiungiamo questo all'angolo precedente per ottenere l'angolo corrente.

#include "Wire.h"
#include "I2Cdev.h" #include "MPU6050.h"

MPU6050 mpu;

int16_t gyroX, gyroRate; float gyroAngle = 0; unsigned long currTime, prevTime = 0, loopTime;

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

void loop () {currTime = millis (); loopTime = currTime - prevTime; prevTime = currTime; gyroX = mpu.getRotationX (); gyroRate = mappa (gyroX, -32768, 32767, -250, 250); gyroAngle = gyroAngle + (float) gyroRate * loopTime / 1000; Serial.println (gyroAngle); }

La posizione dell'MPU6050 all'avvio del programma è il punto di inclinazione zero. L'angolo di inclinazione verrà misurato rispetto a questo punto.

Mantieni il robot fermo ad un angolo fisso e noterai che l'angolo aumenterà o diminuirà gradualmente. Non rimarrà stabile. Ciò è dovuto alla deriva che è inerente al giroscopio.

Nel codice indicato in precedenza, il tempo di ciclo viene calcolato utilizzando la funzione millis () incorporata nell'IDE di Arduino. Nei passaggi successivi, utilizzeremo gli interrupt del timer per creare intervalli di campionamento precisi. Questo periodo di campionamento verrà utilizzato anche per generare l'uscita utilizzando un controller PID.

Passaggio 5: combinazione dei risultati con un filtro complementare

Google definisce complementare come " combinare in modo tale da migliorare o enfatizzare le qualità reciproche o di altro ".

Abbiamo due misure dell'angolo da due diverse fonti. La misurazione dall'accelerometro viene influenzata da improvvisi movimenti orizzontali e la misurazione dal giroscopio si allontana gradualmente dal valore reale. In altre parole, la lettura dell'accelerometro viene influenzata da segnali di breve durata e la lettura del giroscopio da segnali di lunga durata. Queste letture sono, in un certo senso, complementari tra loro. Combinandoli entrambi usando un filtro complementare per ottenere una misurazione dell'angolo stabile e precisa. Il filtro complementare è essenzialmente un filtro passa-alto che agisce sul giroscopio e un filtro passa-basso che agisce sull'accelerometro per filtrare la deriva e il rumore dalla misurazione.

currentAngle = 0.9934 * (previousAngle + gyroAngle) + 0.0066 * (accAngle)

0.9934 e 0.0066 sono coefficienti di filtro per una costante di tempo del filtro di 0, 75 s. Il filtro passa-basso consente il passaggio di qualsiasi segnale più lungo di questa durata e il filtro passa-alto consente il passaggio di qualsiasi segnale più breve di questa durata. La risposta del filtro può essere modificata selezionando la costante di tempo corretta. Abbassare la costante di tempo consentirà di passare più accelerazione orizzontale.

Eliminazione degli errori di offset dell'accelerometro e del giroscopio
Scarica ed esegui il codice indicato in questa pagina per calibrare gli offset di MPU6050. Qualsiasi errore dovuto all'offset può essere eliminato definendo i valori di offset nella routine setup () come mostrato di seguito.

mpu.setYAccelOffset (1593);
mpu.setZAccelOffset (963); mpu.setXGyroOffset (40);

Passaggio 6: controllo PID per la generazione di output

PID è l'acronimo di Proporzionale, integrale e derivata. Ognuno di questi termini fornisce una risposta unica al nostro robot auto-bilanciante.

Il termine proporzionale, come suggerisce il nome, genera una risposta proporzionale all'errore. Per il nostro sistema, l'errore è l'angolo di inclinazione del robot.

Il termine integrale genera una risposta basata sull'errore accumulato. Questa è essenzialmente la somma di tutti gli errori moltiplicati per il periodo di campionamento. Questa è una risposta basata sul comportamento del sistema in passato.

Il termine derivativo è proporzionale alla derivata dell'errore. Questa è la differenza tra l'errore corrente e l'errore precedente diviso per il periodo di campionamento. Questo agisce come un termine predittivo che risponde a come il robot potrebbe comportarsi nel prossimo ciclo di campionamento.

Moltiplicando ciascuno di questi termini per le loro costanti corrispondenti (cioè Kp, Ki e Kd) e sommando il risultato, generiamo l'output che viene quindi inviato come comando per guidare il motore.

Passaggio 7: ottimizzazione delle costanti PID

1. Impostare Ki e Kd su zero e aumentare gradualmente Kp in modo che il robot inizi a oscillare attorno alla posizione zero.

2. Aumentare il Ki in modo che la risposta del robot sia più veloce quando è sbilanciata. Il Ki dovrebbe essere abbastanza grande da non aumentare l'angolo di inclinazione. Il robot dovrebbe tornare alla posizione zero se è inclinato.

3. Aumentare Kd in modo da ridurre le oscillazioni. Ormai anche i superamenti dovrebbero essere ridotti.

4. Ripetere i passaggi precedenti perfezionando ogni parametro per ottenere il risultato migliore.

Passaggio 8: aggiunta del sensore di distanza

Il sensore di distanza ad ultrasuoni che ho usato è l'US-020. Ha quattro pin vale a dire Vcc, Trig, Echo e Gnd. È alimentato da una sorgente 5V. I pin trigger ed echo sono rispettivamente collegati ai pin digitali 9 e 8 di Arduino. Useremo la libreria NewPing per ottenere il valore della distanza dal sensore. Leggeremo la distanza ogni 100 millisecondi e se il valore è compreso tra 0 e 20 cm, comanderemo al robot di eseguire una rotazione. Ciò dovrebbe essere sufficiente per allontanare il robot dall'ostacolo.

Passaggio 9: il codice completo

 #include "Wire.h" 

#include "I2Cdev.h" #include "MPU6050.h" #include "math.h" #include

#define leftMotorPWMPin 6 #define leftMotorDirPin 7 #define rightMotorPWMPin 5 #define rightMotorDirPin 4

#define TRIGGER_PIN 9 #define ECHO_PIN 8 #define MAX_DISTANCE 75

#define Kp 40 #define Kd 0, 05 #define Ki 40 #define sampleTime 0, 005 #define targetAngolo -2.5

MPU6050 mpu; Sonar NewPing (TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);

int16_t accY, accZ, gyroX; int volatile motorPower, gyroRate; float volatile accAngle, gyroAngle, currentAngle, prevAngle = 0, errore, prevError = 0, errorSum = 0; conteggio dei byte volatile = 0; int distanceCm;

void setMotors (int leftMotorSpeed, int rightMotorSpeed) {if (leftMotorSpeed> = 0) {analogWrite (leftMotorPWMPin, leftMotorSpeed); digitalWrite (leftMotorDirPin, LOW); } else {analogWrite (leftMotorPWMPin, 255 + leftMotorSpeed); digitalWrite (leftMotorDirPin, HIGH); } if (rightMotorSpeed> = 0) {analogWrite (rightMotorPWMPin, rightMotorSpeed); digitalWrite (rightMotorDirPin, LOW); } else {analogWrite (rightMotorPWMPin, 255 + rightMotorSpeed); digitalWrite (rightMotorDirPin, HIGH); }}

void init_PID () // inizializza Timer1 cli (); // disabilita gli interrupt globali TCCR1A = 0; // imposta l'intero registro TCCR1A su 0 TCCR1B = 0; // lo stesso per TCCR1B // imposta il registro delle corrispondenze per impostare il tempo di campionamento 5ms OCR1A = 9999; // attiva la modalità CTC TCCR1B

void setup () {// imposta il controllo motore e i pin PWM in modalità output pinMode (leftMotorPWMPin, OUTPUT); pinMode (leftMotorDirPin, OUTPUT); pinMode (rightMotorPWMPin, OUTPUT); pinMode (rightMotorDirPin, OUTPUT); // imposta il LED di stato sulla modalità di uscita pinMode (13, OUTPUT); // inizializza MPU6050 e imposta i valori di offset mpu.initialize (); mpu.setYAccelOffset (1593); mpu.setZAccelOffset (963); mpu.setXGyroOffset (40); // inizializza il ciclo di campionamento PID init_PID (); }

void loop () {// leggi i valori di accelerazione e giroscopio accY = mpu.getAccelerationY (); accZ = mpu.getAccelerationZ (); gyroX = mpu.getRotationX (); // imposta la potenza del motore dopo averlo limitato motorPower = vincolo (motorPower, -255, 255); setMotors (motorPower, motorPower); // misura la distanza ogni 100 millisecondi if ((count% 20) == 0) {distanceCm = sonar.ping_cm (); } if ((distanceCm <20) && (distanceCm! = 0)) {setMotors (-motorPower, motorPower); }} // L'ISR verrà chiamato ogni 5 millisecondi ISR ​​(TIMER1_COMPA_vect) {// calcola l'angolo di inclinazione accAngle = atan2 (accY, accZ) * RAD_TO_DEG; gyroRate = mappa (gyroX, -32768, 32767, -250, 250); gyroAngle = (float) gyroRate * sampleTime; currentAngle = 0.9934 * (prevAngle + gyroAngle) + 0.0066 * (accAngle); errore = currentAngle - targetAngle; errorSum = errorSum + errore; errorSum = vincolo (errorSum, -300, 300); // calcola l'output dai valori P, I e D motorPower = Kp * (errore) + Ki * (errorSum) * sampleTime - Kd * (currentAngle-prevAngle) / sampleTime; prevAngle = currentAngle; // attiva / disattiva il led su pin13 ogni secondo conteggio ++; if (count == 200) {count = 0; digitalWrite (13, ! digitalRead (13)); }}

Passaggio 10: pensieri finali

Trascorrere un po 'più di tempo a modificare le costanti PID ci darebbe un risultato migliore. Le dimensioni del nostro robot limitano anche il livello di stabilità che possiamo raggiungere. È più facile costruire un robot di bilanciamento a grandezza naturale piuttosto che costruirne uno piccolo come il nostro. Tuttavia, immagino, il nostro robot fa un lavoro abbastanza decente nell'equilibrare su varie superfici come mostrato nel video.

Per ora è tutto.

Grazie per il tuo tempo. Non dimenticare di lasciare i tuoi pensieri nella sezione commenti.

Articoli Correlati