Robot Arduino à équilibrage automatique

Bonjour à tous!

Dans cet instructable, je vais vous montrer comment construire un petit robot auto-équilibré qui peut se déplacer en évitant les obstacles. Il s'agit d'un petit robot mesurant 4 pouces de large et 4 pouces de haut et est basé sur la carte de développement Arduino Pro Mini et le module accéléromètre-gyroscope MPU6050.

Dans les étapes qui suivent, nous verrons comment interfacer le MPU6050 avec Arduino, comment mesurer l'angle d'inclinaison du robot, comment utiliser le PID pour que le robot reste équilibré. Un télémètre à ultrasons est également ajouté au robot, ce qui l'empêche de cogner contre les obstacles lorsqu'il se promène.

Liste des pièces

J'ai acheté la plupart de ces pièces chez aliexpress, mais vous pouvez également les trouver dans n'importe quel autre magasin d'électronique.

1. Arduino Pro Mini

2. Module GY-521 avec MPU-6050

3. Pilote de moteur DRV8833 Pololu

4. Convertisseur boost 2, 5V

5. Capteur de distance à ultrasons US-020

6. Batterie et support NCR18650

7. Paire de micromoteurs à engrenages métalliques (N20, 6V, 200 tr / min) et supports

8. Paire de roues 42x19mm

9. 3, PCB prototype double face (4 cm x 6 cm)

10. 8, entretoises en nylon de 25 cm et 4, écrous en nylon

En dehors de ce qui précède, vous aurez besoin de quelques câbles, connecteurs berg et un interrupteur marche / arrêt.

Étape 1: Un peu de théorie

Commençons par quelques fondamentaux avant de nous salir les mains.

Le robot auto-équilibré est similaire à un pendule à l'envers. Contrairement à un pendule normal qui continue de se balancer une fois que l'on lui donne un coup de pouce, ce pendule inversé ne peut pas rester équilibré seul. Il tombera simplement. Alors, comment pouvons-nous équilibrer cela? Envisagez d'équilibrer un balai sur notre index, qui est un exemple classique d'équilibrage d'un pendule inversé. Nous déplaçons notre doigt dans la direction où le bâton tombe. Il en va de même avec un robot à équilibrage automatique, seulement que le robot tombera soit vers l'avant, soit vers l'arrière. Tout comme la façon dont nous équilibrons un bâton sur notre doigt, nous équilibrons le robot en entraînant ses roues dans la direction dans laquelle il tombe. Ce que nous essayons de faire ici, c'est de maintenir le centre de gravité du robot exactement au-dessus du point de pivot.

Pour entraîner les moteurs, nous avons besoin d'informations sur l'état du robot. Nous devons connaître la direction dans laquelle le robot tombe, combien le robot s'est incliné et la vitesse à laquelle il tombe. Toutes ces informations peuvent être déduites des lectures obtenues à partir du MPU6050. Nous combinons toutes ces entrées et générons un signal qui entraîne les moteurs et maintient le robot en équilibre.

Étape 2: Commençons à construire

Nous terminerons d'abord les circuits et la structure du robot. Le robot est construit sur trois couches de perfboards espacées de 25 mm à l'aide d'entretoises en nylon. La couche inférieure contient les deux moteurs et le pilote de moteur. La couche intermédiaire comprend le contrôleur, l'IMU et les modules de régulateur de suralimentation 5V. La couche supérieure a la batterie, un interrupteur marche / arrêt et le capteur de distance à ultrasons (nous l'installerons vers la fin une fois que le robot sera en équilibre).

Avant de commencer à prototyper sur une planche de perfusion, nous devons avoir une image claire de l'endroit où chaque pièce doit être placée. Pour faciliter le prototypage, il est toujours préférable de dessiner la disposition physique de tous les composants et de l'utiliser comme référence pour placer les composants et acheminer les cavaliers sur la perfboard. Une fois que toutes les pièces sont placées et soudées, interconnectez les trois planches à l'aide d'entretoises en nylon.

Vous avez peut-être remarqué que j'ai utilisé deux modules de régulateur de tension séparés pour piloter les moteurs et le contrôleur, même s'ils nécessitent tous deux une source de 5 V. C'est très important. Dans ma première conception, j'ai utilisé un seul régulateur de suralimentation 5V pour alimenter le contrôleur ainsi que les moteurs. Lorsque j'ai allumé le robot, le programme se bloque par intermittence. Cela était dû au bruit généré par le circuit du moteur agissant sur le contrôleur et l'IMU. Cela a été efficacement éliminé en séparant le régulateur de tension du contrôleur et du moteur et en ajoutant un condensateur de 10 uF aux bornes d'alimentation du moteur.

Étape 3: mesure de l'angle d'inclinaison à l'aide d'un accéléromètre

Le MPU6050 possède un accéléromètre à 3 axes et un gyroscope à 3 axes. L'accéléromètre mesure l'accélération le long des trois axes et le gyroscope mesure la vitesse angulaire autour des trois axes. Pour mesurer l'angle d'inclinaison du robot, nous avons besoin de valeurs d'accélération le long des axes y et z. La fonction atan2 (y, z) donne l'angle en radians entre l'axe z positif d'un plan et le point donné par les coordonnées (z, y) sur ce plan, avec un signe positif pour les angles antihoraires (demi-droite plan, y> 0) et signe négatif pour les angles dans le sens horaire (demi-plan gauche, y <0). Nous utilisons cette bibliothèque écrite par Jeff Rowberg pour lire les données du MPU6050. Téléchargez le code ci-dessous et voyez comment l'angle d'inclinaison varie.

#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); }

boucle vide () {accZ = mpu.getAccelerationZ (); accY = mpu.getAccelerationY (); accAngle = atan2 (accY, accZ) * RAD_TO_DEG; if (isnan (accAngle)); else Serial.println (accAngle); }

Essayez de déplacer le robot vers l'avant et vers l'arrière tout en le maintenant incliné à un angle fixe. Vous remarquerez que l'angle affiché sur votre moniteur série change soudainement. Cela est dû à la composante horizontale de l'accélération qui interfère avec les valeurs d'accélération des axes y et z.

Étape 4: mesure de l'angle d'inclinaison à l'aide d'un gyroscope

Le gyroscope à 3 axes du MPU6050 mesure la vitesse angulaire (vitesse de rotation) le long des trois axes. Pour notre robot auto-équilibré, la vitesse angulaire le long de l'axe des x est suffisante pour mesurer la vitesse de chute du robot.

Dans le code ci-dessous, nous lisons la valeur du gyroscope sur l'axe des x, la convertissons en degrés par seconde, puis la multiplions par le temps de boucle pour obtenir le changement d'angle. Nous ajoutons cela à l'angle précédent pour obtenir l'angle actuel.

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

MPU6050 mpu;

int16_t gyroX, gyroRate; float gyroAngle = 0; long currTime non signé, prevTime = 0, loopTime;

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

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

La position du MPU6050 lorsque le programme démarre est le point d'inclinaison nul. L'angle d'inclinaison sera mesuré par rapport à ce point.

Gardez le robot stable à un angle fixe et vous observerez que l'angle augmentera ou diminuera progressivement. Cela ne restera pas stable. Cela est dû à la dérive inhérente au gyroscope.

Dans le code donné ci-dessus, le temps de boucle est calculé en utilisant la fonction millis () qui est intégrée dans l'IDE Arduino. Dans les étapes ultérieures, nous utiliserons des interruptions de minuterie pour créer des intervalles d'échantillonnage précis. Cette période d'échantillonnage sera également utilisée pour générer la sortie à l'aide d'un contrôleur PID.

Étape 5: Combiner les résultats avec un filtre complémentaire

Google définit la complémentarité comme «se combinant de manière à améliorer ou à mettre en valeur les qualités des uns et des autres ».

Nous avons deux mesures de l'angle à partir de deux sources différentes. La mesure de l'accéléromètre est affectée par des mouvements horizontaux soudains et la mesure du gyroscope s'éloigne progressivement de la valeur réelle. En d'autres termes, la lecture de l'accéléromètre est affectée par des signaux de courte durée et la lecture du gyroscope par des signaux de longue durée. Ces lectures sont en quelque sorte complémentaires les unes des autres. Combinez-les tous les deux à l'aide d'un filtre complémentaire et nous obtenons une mesure stable et précise de l'angle. Le filtre complémentaire est essentiellement un filtre passe-haut agissant sur le gyroscope et un filtre passe-bas agissant sur l'accéléromètre pour filtrer la dérive et le bruit de la mesure.

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

0.9934 et 0.0066 sont des coefficients de filtre pour une constante de temps de filtre de 0.75s. Le filtre passe-bas laisse passer tout signal plus long que cette durée et le filtre passe-haut laisse passer tout signal plus court que cette durée. La réponse du filtre peut être modifiée en choisissant la constante de temps correcte. L'abaissement de la constante de temps permettra à plus d'accélération horizontale de passer.

Élimination des erreurs de décalage de l'accéléromètre et du gyroscope
Téléchargez et exécutez le code donné dans cette page pour calibrer les décalages du MPU6050. Toute erreur due au décalage peut être éliminée en définissant les valeurs de décalage dans la routine setup () comme indiqué ci-dessous.

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

Étape 6: Contrôle PID pour générer une sortie

PID signifie Proportionnel, Intégral et Dérivé. Chacun de ces termes apporte une réponse unique à notre robot auto-équilibré.

Le terme proportionnel, comme son nom l'indique, génère une réponse proportionnelle à l'erreur. Pour notre système, l'erreur est l'angle d'inclinaison du robot.

Le terme intégral génère une réponse basée sur l'erreur accumulée. Il s'agit essentiellement de la somme de toutes les erreurs multipliée par la période d'échantillonnage. Il s'agit d'une réponse basée sur le comportement du système dans le passé.

Le terme dérivé est proportionnel à la dérivée de l'erreur. Il s'agit de la différence entre l'erreur actuelle et l'erreur précédente divisée par la période d'échantillonnage. Cela agit comme un terme prédictif qui répond à la façon dont le robot pourrait se comporter dans la prochaine boucle d'échantillonnage.

En multipliant chacun de ces termes par leurs constantes correspondantes (c'est-à-dire Kp, Ki et Kd) et en additionnant le résultat, nous générons la sortie qui est ensuite envoyée comme commande pour entraîner le moteur.

Étape 7: réglage des constantes PID

1. Réglez Ki et Kd sur zéro et augmentez progressivement Kp pour que le robot commence à osciller autour de la position zéro.

2. Augmentez Ki pour que la réponse du robot soit plus rapide lorsqu'il est déséquilibré. Ki doit être suffisamment grand pour que l'angle d'inclinaison n'augmente pas. Le robot doit revenir en position zéro s'il est incliné.

3. Augmentez Kd pour réduire les oscillations. Les dépassements devraient également être réduits d'ici là.

4. Répétez les étapes ci-dessus en affinant chaque paramètre pour obtenir le meilleur résultat.

Étape 8: Ajout du capteur de distance

Le capteur de distance à ultrasons que j'ai utilisé est le US-020. Il a quatre broches à savoir Vcc, Trig, Echo et Gnd. Il est alimenté par une source 5V. Les broches de déclenchement et d'écho sont respectivement connectées aux broches numériques 9 et 8 d'Arduino. Nous utiliserons la bibliothèque NewPing pour obtenir la valeur de distance du capteur. Nous lirons la distance une fois toutes les 100 millisecondes et si la valeur est comprise entre 0 et 20 cm, nous commanderons au robot d'effectuer une rotation. Cela devrait être suffisant pour éloigner le robot de l'obstacle.

Étape 9: Le code complet

 #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 targetAngle -2, 5

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

int16_t accY, accZ, gyroX; volatile int motorPower, gyroRate; flottant flottant accAngle, gyroAngle, currentAngle, prevAngle = 0, error, prevError = 0, errorSum = 0; nombre d'octets volatils = 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 () // initialise Timer1 cli (); // désactive les interruptions globales TCCR1A = 0; // met l'ensemble du registre TCCR1A à 0 TCCR1B = 0; // idem pour TCCR1B // définir le registre de correspondance de comparaison pour définir le temps d'échantillonnage 5 ms OCR1A = 9999; // activer le mode CTC TCCR1B

void setup () {// définit la commande du moteur et les broches PWM en mode de sortie pinMode (leftMotorPWMPin, OUTPUT); pinMode (leftMotorDirPin, OUTPUT); pinMode (rightMotorPWMPin, OUTPUT); pinMode (rightMotorDirPin, OUTPUT); // règle la LED d'état sur le mode de sortie pinMode (13, OUTPUT); // initialise le MPU6050 et définit les valeurs de décalage mpu.initialize (); mpu.setYAccelOffset (1593); mpu.setZAccelOffset (963); mpu.setXGyroOffset (40); // initialise la boucle d'échantillonnage PID init_PID (); }

void loop () {// lire les valeurs d'accélération et de gyroscope accY = mpu.getAccelerationY (); accZ = mpu.getAccelerationZ (); gyroX = mpu.getRotationX (); // définit la puissance du moteur après l'avoir contraint motorPower = contrainte (motorPower, -255, 255); setMotors (motorPower, motorPower); // mesure la distance toutes les 100 millisecondes si ((count% 20) == 0) {distanceCm = sonar.ping_cm (); } if ((distanceCm <20) && (distanceCm! = 0)) {setMotors (-motorPower, motorPower); }} // L'ISR sera appelé toutes les 5 millisecondes ISR (TIMER1_COMPA_vect) {// calculer l'angle d'inclinaison accAngle = atan2 (accY, accZ) * RAD_TO_DEG; gyroRate = carte (gyroX, -32768, 32767, -250, 250); gyroAngle = (float) gyroRate * sampleTime; currentAngle = 0.9934 * (prevAngle + gyroAngle) + 0.0066 * (accAngle); error = currentAngle - targetAngle; errorSum = errorSum + error; errorSum = contrainte (errorSum, -300, 300); // calculer la sortie à partir des valeurs P, I et D motorPower = Kp * (error) + Ki * (errorSum) * sampleTime - Kd * (currentAngle-prevAngle) / sampleTime; prevAngle = currentAngle; // bascule la led sur la broche 13 tous les seconds count ++; if (count == 200) {count = 0; digitalWrite (13, ! digitalRead (13)); }}

Étape 10: Réflexions finales

Passer un peu plus de temps à peaufiner les constantes PID nous donnerait un meilleur résultat. La taille de notre robot limite également le niveau de stabilité que nous pouvons atteindre. Il est plus facile de construire un robot d'équilibrage en taille réelle que d'en construire un petit comme le nôtre. Pourtant, je suppose que notre robot fait un travail assez décent en équilibrant sur diverses surfaces comme le montre la vidéo.

C'est tout pour le moment.

Merci pour votre temps. N'oubliez pas de laisser vos pensées dans la section des commentaires.

Articles Connexes