Hallo zusammen!

In dieser Anleitung zeige ich Ihnen, wie Sie einen kleinen selbstausgleichenden Roboter bauen, der sich bewegen kann, um Hindernissen auszuweichen. Dies ist ein winziger Roboter mit einer Breite von 4 Zoll und einer Höhe von 4 Zoll. Er basiert auf dem Arduino Pro Mini-Entwicklungsboard und dem Beschleunigungsmesser-Gyroskop-Modul MPU6050.

In den folgenden Schritten erfahren Sie, wie Sie die MPU6050 mit Arduino verbinden, den Neigungswinkel des Roboters messen und die PID verwenden, um den Roboter im Gleichgewicht zu halten. Dem Roboter ist außerdem ein Ultraschall-Entfernungsmesser hinzugefügt, der verhindert, dass er beim Herumwandern gegen Hindernisse stößt.

Liste der Einzelteile

Ich habe die meisten dieser Teile bei aliexpress gekauft, aber Sie können sie auch in jedem anderen Elektronikgeschäft finden.

1. Arduino Pro Mini

2. GY-521-Modul mit MPU-6050

3. DRV8833 Pololu Motortreiber

4. 2, 5V Aufwärtswandler

5. Ultraschall-Abstandssensor US-020

6. NCR18650 Batterie und Halter

7. Paar Mikrometallgetriebemotoren (N20, 6V, 200 U / min) und Halterungen

8. Paar 42x19mm Räder

9. 3, doppelseitige Prototyp-Leiterplatte (4 cm x 6 cm)

10. 8, 25 cm Nylonabstandshalter und 4 Nylonmuttern

Abgesehen davon benötigen Sie einige Kabel, Bergstecker und einen Ein / Aus-Schalter.

Schritt 1: Ein bisschen Theorie

Beginnen wir mit einigen Grundlagen, bevor wir uns die Hände schmutzig machen.

Der selbstausgleichende Roboter ähnelt einem umgedrehten Pendel. Im Gegensatz zu einem normalen Pendel, das nach einem Schub weiter schwingt, kann dieses umgekehrte Pendel nicht alleine ausbalanciert bleiben. Es wird einfach umfallen. Wie balancieren wir es dann? Ziehen Sie in Betracht, einen Besenstiel auf unserem Zeigefinger zu balancieren. Dies ist ein klassisches Beispiel für das Balancieren eines umgekehrten Pendels. Wir bewegen unseren Finger in die Richtung, in die der Stock fällt. Ähnlich ist es bei einem selbstausgleichenden Roboter, nur dass der Roboter entweder vorwärts oder rückwärts fällt. Genau wie wir einen Stock an unserem Finger balancieren, balancieren wir den Roboter, indem wir seine Räder in die Richtung fahren, in die er fällt. Wir versuchen hier, den Schwerpunkt des Roboters genau über dem Drehpunkt zu halten.

Um die Motoren anzutreiben, benötigen wir einige Informationen über den Zustand des Roboters. Wir müssen wissen, in welche Richtung der Roboter fällt, wie stark der Roboter geneigt ist und mit welcher Geschwindigkeit er fällt. Alle diese Informationen können aus den Messwerten der MPU6050 abgeleitet werden. Wir kombinieren all diese Eingänge und erzeugen ein Signal, das die Motoren antreibt und den Roboter im Gleichgewicht hält.

Schritt 2: Beginnen wir mit dem Bauen

Wir werden zuerst die Schaltung und Struktur des Roboters vervollständigen. Der Roboter besteht aus drei Schichten Perfboard, die mit Nylon-Abstandshaltern einen Abstand von 25 mm haben. Die untere Schicht enthält die beiden Motoren und den Motortreiber. Die mittlere Schicht besteht aus dem Controller, der IMU und den 5-V-Boost-Reglermodulen. Die oberste Schicht besteht aus der Batterie, einem Ein / Aus-Schalter und dem Ultraschall-Abstandssensor (wir werden diesen gegen Ende installieren, sobald der Roboter das Gleichgewicht erreicht hat).

Bevor wir mit dem Prototyping auf einem Perfboard beginnen, sollten wir ein klares Bild davon haben, wo jedes Teil platziert werden sollte. Um das Prototyping zu vereinfachen, ist es immer besser, das physische Layout aller Komponenten zu zeichnen und dies als Referenz zu verwenden, um die Komponenten zu platzieren und die Jumper auf dem Perfboard zu routen. Sobald alle Teile platziert und verlötet sind, verbinden Sie die drei Platinen mit Nylon-Abstandshaltern.

Sie haben vielleicht bemerkt, dass ich zwei separate Spannungsreglermodule zum Antreiben der Motoren und des Controllers verwendet habe, obwohl beide eine 5-V-Quelle benötigen. Dies ist sehr wichtig. In meinem ersten Entwurf habe ich einen einzelnen 5-V-Boost-Regler verwendet, um sowohl den Controller als auch die Motoren einzuschalten. Wenn ich den Roboter einschalte, friert das Programm zeitweise ein. Dies war auf das Rauschen zurückzuführen, das von der auf die Steuerung und die IMU einwirkenden Motorschaltung erzeugt wurde. Dies wurde effektiv beseitigt, indem der Spannungsregler von der Steuerung und dem Motor getrennt und ein 10 uF-Kondensator an den Motorstromversorgungsklemmen hinzugefügt wurde.

Schritt 3: Neigungswinkel mit dem Beschleunigungsmesser messen

Die MPU6050 verfügt über einen 3-Achsen-Beschleunigungsmesser und ein 3-Achsen-Gyroskop. Der Beschleunigungsmesser misst die Beschleunigung entlang der drei Achsen und das Gyroskop misst die Winkelgeschwindigkeit um die drei Achsen. Um den Neigungswinkel des Roboters zu messen, benötigen wir Beschleunigungswerte entlang der y- und z-Achse. Die Funktion atan2 (y, z) gibt den Winkel im Bogenmaß zwischen der positiven z-Achse einer Ebene und dem Punkt an, der durch die Koordinaten (z, y) auf dieser Ebene gegeben ist, mit positivem Vorzeichen für Winkel gegen den Uhrzeigersinn (rechte Hälfte) Ebene, y> 0) und negatives Vorzeichen für Winkel im Uhrzeigersinn (linke Halbebene, y <0). Wir verwenden diese von Jeff Rowberg geschriebene Bibliothek, um die Daten von MPU6050 zu lesen. Laden Sie den unten angegebenen Code hoch und sehen Sie, wie sich der Neigungswinkel ändert.

#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; if (isnan (accAngle)); sonst Serial.println (accAngle); }}

Versuchen Sie, den Roboter vorwärts und rückwärts zu bewegen, während Sie ihn in einem festen Winkel geneigt halten. Sie werden feststellen, dass sich der in Ihrem seriellen Monitor angezeigte Winkel plötzlich ändert. Dies ist auf die horizontale Beschleunigungskomponente zurückzuführen, die die Beschleunigungswerte der y- und z-Achse stört.

Schritt 4: Neigungswinkel mit dem Gyroskop messen

Das 3-Achsen-Gyroskop der MPU6050 misst die Winkelgeschwindigkeit (Rotationsgeschwindigkeit) entlang der drei Achsen. Für unseren selbstausgleichenden Roboter reicht die Winkelgeschwindigkeit entlang der x-Achse allein aus, um die Fallrate des Roboters zu messen.

In dem unten angegebenen Code lesen wir den Kreiselwert um die x-Achse, konvertieren ihn in Grad pro Sekunde und multiplizieren ihn dann mit der Schleifenzeit, um die Winkeländerung zu erhalten. Wir addieren dies zum vorherigen Winkel, um den aktuellen Winkel zu erhalten.

#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 = map (gyroX, -32768, 32767, -250, 250); gyroAngle = gyroAngle + (float) gyroRate * loopTime / 1000; Serial.println (gyroAngle); }}

Die Position der MPU6050 zu Beginn des Programms ist der Nullneigungspunkt. Der Neigungswinkel wird in Bezug auf diesen Punkt gemessen.

Halten Sie den Roboter in einem festen Winkel ruhig und Sie werden feststellen, dass der Winkel allmählich zunimmt oder abnimmt. Es wird nicht stabil bleiben. Dies ist auf die Drift zurückzuführen, die dem Gyroskop innewohnt.

In dem oben angegebenen Code wird die Schleifenzeit mit der in die Arduino-IDE integrierten Funktion millis () berechnet. In späteren Schritten werden wir Timer-Interrupts verwenden, um präzise Abtastintervalle zu erstellen. Diese Abtastperiode wird auch zur Erzeugung des Ausgangs unter Verwendung eines PID-Reglers verwendet.

Schritt 5: Kombinieren der Ergebnisse mit einem ergänzenden Filter

Google definiert Komplementär als "so kombinieren, dass die Eigenschaften des anderen oder eines anderen verbessert oder hervorgehoben werden ".

Wir haben zwei Messungen des Winkels aus zwei verschiedenen Quellen. Die Messung vom Beschleunigungsmesser wird durch plötzliche horizontale Bewegungen beeinflusst und die Messung vom Gyroskop weicht allmählich vom tatsächlichen Wert ab. Mit anderen Worten wird die Beschleunigungsmesserablesung durch Signale von kurzer Dauer und die Gyroskopablesung durch Signale von langer Dauer beeinflusst. Diese Messwerte ergänzen sich in gewisser Weise. Kombinieren Sie beide mit einem Komplementärfilter und Sie erhalten eine stabile, genaue Messung des Winkels. Das Komplementärfilter ist im Wesentlichen ein Hochpassfilter, das auf das Gyroskop wirkt, und ein Tiefpassfilter, das auf den Beschleunigungsmesser wirkt, um die Drift und das Rauschen aus der Messung herauszufiltern.

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

0, 9934 und 0, 0066 sind Filterkoeffizienten für eine Filterzeitkonstante von 0, 75 s. Das Tiefpassfilter lässt jedes Signal, das länger als diese Dauer ist, durch und das Hochpassfilter lässt jedes Signal durch, das kürzer als diese Dauer ist. Die Reaktion des Filters kann durch Auswahl der richtigen Zeitkonstante optimiert werden. Durch Verringern der Zeitkonstante kann mehr horizontale Beschleunigung durchlaufen werden.

Beseitigung von Versatzfehlern bei Beschleunigungsmesser und Gyroskop
Laden Sie den auf dieser Seite angegebenen Code herunter und führen Sie ihn aus, um die Offsets der MPU6050 zu kalibrieren. Jeder Fehler aufgrund eines Versatzes kann behoben werden, indem die Versatzwerte in der setup () -Routine wie unten gezeigt definiert werden.

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

Schritt 6: PID-Regelung zur Stromerzeugung

PID steht für Proportional, Integral und Derivative. Jeder dieser Begriffe bietet eine einzigartige Antwort auf unseren selbstausgleichenden Roboter.

Der proportionale Term erzeugt, wie der Name schon sagt, eine Antwort, die proportional zum Fehler ist. Bei unserem System ist der Fehler der Neigungswinkel des Roboters.

Der Integralterm erzeugt eine Antwort basierend auf dem akkumulierten Fehler. Dies ist im Wesentlichen die Summe aller Fehler multipliziert mit der Abtastperiode. Dies ist eine Antwort, die auf dem Verhalten des Systems in der Vergangenheit basiert.

Der Ableitungsterm ist proportional zur Ableitung des Fehlers. Dies ist die Differenz zwischen dem aktuellen Fehler und dem vorherigen Fehler geteilt durch die Abtastperiode. Dies fungiert als prädiktiver Begriff, der darauf reagiert, wie sich der Roboter in der nächsten Abtastschleife verhält.

Multipliziert man jeden dieser Terme mit den entsprechenden Konstanten (dh Kp, Ki und Kd) und summiert das Ergebnis, so wird die Ausgabe generiert, die dann als Befehl zum Antreiben des Motors gesendet wird.

Schritt 7: Einstellen der PID-Konstanten

1. Setzen Sie Ki und Kd auf Null und erhöhen Sie Kp allmählich, so dass der Roboter beginnt, um die Nullposition zu schwingen.

2. Erhöhen Sie Ki, damit der Roboter schneller reagiert, wenn er nicht im Gleichgewicht ist. Ki sollte groß genug sein, damit sich der Neigungswinkel nicht vergrößert. Der Roboter sollte in die Nullposition zurückkehren, wenn er geneigt ist.

3. Erhöhen Sie Kd, um die Schwingungen zu verringern. Die Überschwinger sollten inzwischen ebenfalls reduziert werden.

4. Wiederholen Sie die obigen Schritte, indem Sie jeden Parameter fein einstellen, um das beste Ergebnis zu erzielen.

Schritt 8: Hinzufügen des Abstandssensors

Der Ultraschall-Abstandssensor, den ich verwendet habe, ist der US-020. Es hat vier Pins, nämlich Vcc, Trig, Echo und Gnd. Es wird von einer 5V-Quelle gespeist. Die Trigger- und Echo-Pins sind jeweils mit den digitalen Pins 9 und 8 von Arduino verbunden. Wir werden die NewPing-Bibliothek verwenden, um den Abstandswert vom Sensor zu erhalten. Wir werden die Entfernung alle 100 Millisekunden ablesen und wenn der Wert zwischen 0 und 20 cm liegt, werden wir dem Roboter befehlen, eine Drehung durchzuführen. Dies sollte ausreichen, um den Roboter vom Hindernis wegzulenken.

Schritt 9: Der vollständige Code

 #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; NewPing-Sonar (TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);
int16_t accY, accZ, gyroX; flüchtig int motorPower, gyroRate; volatile float accAngle, gyroAngle, currentAngle, prevAngle = 0, error, prevError = 0, errorSum = 0; Anzahl flüchtiger Bytes = 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 () // initialisiere Timer1 cli (); // globale Interrupts deaktivieren TCCR1A = 0; // setze das gesamte TCCR1A Register auf 0 TCCR1B = 0; // Gleiches gilt für TCCR1B // Setze das Vergleichs-Match-Register, um die Abtastzeit auf 5 ms einzustellen. OCR1A = 9999; // CTC-Modus TCCR1B einschalten
void setup () {// setze die Motorsteuerung und die PWM-Pins auf den Ausgangsmodus pinMode (leftMotorPWMPin, OUTPUT); pinMode (leftMotorDirPin, OUTPUT); pinMode (rightMotorPWMPin, OUTPUT); pinMode (rightMotorDirPin, OUTPUT); // setze die Status LED auf den Ausgangsmodus pinMode (13, OUTPUT); // initialisiere die MPU6050 und setze Offset Werte mpu.initialize (); mpu.setYAccelOffset (1593); mpu.setZAccelOffset (963); mpu.setXGyroOffset (40); // PID-Abtastschleife initialisieren init_PID (); }}
void loop () {// Beschleunigungs- und Gyroskopwerte lesen accY = mpu.getAccelerationY (); accZ = mpu.getAccelerationZ (); gyroX = mpu.getRotationX (); // Motorleistung nach Einschränkung einstellen motorPower = constrain (motorPower, -255, 255); setMotors (motorPower, motorPower); // Entfernung alle 100 Millisekunden messen if ((count% 20) == 0) {distanceCm = sonar.ping_cm (); } if ((distanceCm <20) && (distanceCm! = 0)) {setMotors (-motorPower, motorPower); }} // Der ISR wird alle 5 Millisekunden aufgerufen. ISR (TIMER1_COMPA_vect) {// Neigungswinkel berechnen accAngle = atan2 (accY, accZ) * RAD_TO_DEG; gyroRate = map (gyroX, -32768, 32767, -250, 250); gyroAngle = (float) gyroRate * sampleTime; currentAngle = 0, 9934 * (prevAngle + gyroAngle) + 0, 0066 * (accAngle); error = currentAngle - targetAngle; errorSum = errorSum + error; errorSum = constrain (errorSum, -300, 300); // Ausgabe aus P-, I- und D-Werten berechnen motorPower = Kp * (Fehler) + Ki * (Fehlersumme) * sampleTime - Kd * (currentAngle-prevAngle) / sampleTime; prevAngle = currentAngle; // schalte die LED an Pin13 jede Sekunde um ++; if (count == 200) {count = 0; digitalWrite (13, ! digitalRead (13)); }}

Schritt 10: Letzte Gedanken

Wenn Sie etwas mehr Zeit für die Optimierung der PID-Konstanten aufwenden, erhalten Sie ein besseres Ergebnis. Die Größe unseres Roboters schränkt auch die Stabilität ein, die wir erreichen können. Es ist einfacher, einen Auswuchtroboter in voller Größe zu bauen, als einen kleinen wie unseren. Trotzdem, denke ich, macht unser Roboter einen ziemlich guten Job beim Balancieren auf verschiedenen Oberflächen, wie im Video gezeigt.

Das war es fürs Erste.

Vielen Dank für Ihre Zeit. Vergiss nicht, deine Gedanken im Kommentarbereich zu hinterlassen.