MIRO ist eine offene Indoor-Roboterplattform. Teil 5 - Softwarekomponente: ARDUINO (AVR), wir klettern "unter der Haube"

Schauen wir uns diesmal die Implementierung einiger Schlüsselbibliotheksmethoden für ARDUINO (AVR) an, die für das Bewegen des MIRO-Roboters verantwortlich sind. Dieser Teil wird für alle interessant sein, die sich fragen, wie die Linear- und Winkelgeschwindigkeit des Roboters auf der ARDUINO gesteuert werden kann, der mit Motoren mit den einfachsten Encodern ausgestattet ist.

Inhaltsverzeichnis: Teil 1 , Teil 2 , Teil 3 , Teil 4 , Teil 5 .

Die Methoden, die für das Fahren mit dem Kilometerzähler verantwortlich sind, sind immer noch ein Problem, wenn es darum geht zu erklären, wie, was und warum. Das Erste, was Sie über die Steuerung der Roboterbewegung wissen müssen, ist die einfache und offensichtliche Tatsache, dass die Kollektormotoren des Roboters ohne zusätzliche Einstellung niemals mit der gleichen Geschwindigkeit rotieren. Unterschiedliche Kupplung, unterschiedliche Leistungseigenschaften der Fahrerkanäle, leicht unterschiedliche Elektromotoren und Schmierung im Getriebe.

Die zweite Tatsache, die Sie verstehen und kennen sollten, ist das Vorhandensein von Trägheit im Motor, selbst bei einer ausreichend großen Übersetzung. Das heißt Wenn die Spannung an den Motorklemmen unterbrochen wird, bewegt sich das Rad, auch wenn es nicht belastet ist, um einige Grad weiter. Die Größe dieser zusätzlichen Drehung hängt von der Belastungskraft auf das Rad, von der Drehzahl vor dem Abbau der Belastung und von denselben unsichtbaren Faktoren wie der Art und Menge des Schmiermittels im Getriebe ab.

Diese Tatsachen bestimmen die Implementierung einer Gruppe von Methoden in Bezug auf die Bewegung eines Fahrgestells, das mit Kilometerzählersensoren ausgestattet ist (im Fall von MIRO digitale Codierer für jedes Rad).

Wie wir im vierten Teil herausgefunden haben, gibt es im Softwaremodell die Chassis- Klasse, die die Rotationssteuerung einzelner Chassis-Motoren implementiert. Ich möchte betonen - nicht die Steuerung der Bewegung des Fahrgestells, des Wagens, sondern die Steuerung der Motoren des Wagens. Die direkte Steuerung des Wagens ist in den Klassen Robot und Miro implementiert.

Fangen wir von oben an. Im Folgenden sehen Sie eine Methode der Miro- Klasse, die die Bewegung eines Roboters um eine bestimmte Distanz ( dist , meter ) mit einer bestimmten linearen Geschwindigkeit ( lin_speed , m / s) und Winkelgeschwindigkeit ( ang_speed , deg / s) implementiert . Der Parameter en_break wird derzeit noch nicht berücksichtigt.

int Miro::moveDist(float lin_speed, float ang_speed, float dist, bool en_break) { float _wheelSetAngSpeed[WHEEL_COUNT]; _wheelSetAngSpeed[LEFT] = MIRO_PI2ANG * (lin_speed - (ROBOT_DIAMETER * ang_speed / (2 * MIRO_PI2ANG))) / WHEEL_RADIUS; _wheelSetAngSpeed[RIGHT] = MIRO_PI2ANG * (lin_speed + (ROBOT_DIAMETER * ang_speed / (2 * MIRO_PI2ANG))) / WHEEL_RADIUS; float _wheelSetAng[WHEEL_COUNT]; _wheelSetAng[RIGHT] = _wheelSetAngSpeed[RIGHT] * dist / lin_speed; _wheelSetAng[LEFT] = _wheelSetAngSpeed[LEFT] * dist / lin_speed; return this->chassis.wheelRotateAng(_wheelSetAngSpeed, _wheelSetAng, en_break); } 

Bei dieser Methode werden zunächst die ERFORDERLICHEN Winkelgeschwindigkeiten für den linken und den rechten Motor berechnet. Nach ziemlich offensichtlichen Formeln ist das kein Problem, abzuleiten. Es ist lediglich zu beachten, dass die lineare Geschwindigkeit in der Methode in Metern pro Sekunde und die Winkelgeschwindigkeit in Grad pro Sekunde (nicht im Bogenmaß) angegeben wird. Daher berechnen wir die Konstante MIRO_PI2ANG = 57,29 = 180 / pi vor. ROBOT_DIAMETER - Abstand zwischen dem linken und rechten Rad des Roboters (in Metern), WHEEL_RADIUS - Radius (auch in Metern). Alle numerischen Konstanten für solche Fälle sind in der Datei defs.h enthalten, und die benutzerdefinierten Parameter für Roboter und Chassis befinden sich in der Datei config.h.

Danach wird der Winkel berechnet, um den jedes Rad gedreht werden muss, damit der Roboter die Distanz dist (auch in Metern) zurücklegt.

In diesem Stadium erhalten wir also, mit welcher Geschwindigkeit und in welchem ​​Winkel Sie jedes Rad des Roboterchassis drehen müssen. Anschließend wird die wheelRotateAng () -Methode des Gehäuseobjekts aufgerufen.

Die wheelRotateAng- Methode (float * speed, float * ang, bool en_break) wird verwendet, um die Roboterräder mit den Winkelgeschwindigkeiten, die vom speed [] -Array (in m / s) angegeben werden, um die Winkel zu drehen, die vom ang [] -Array (in Grad) angegeben werden. Der letzte Parameter en_break (den wir bereits früher getroffen haben) legt die Anforderung für ein hartes Anhalten der Räder nach einer Kurvenfahrt fest, indem eine kurzfristige Rückwärtsspannung an sie angelegt wird. Dies ist erforderlich, um die Trägheit des Roboters zu unterdrücken und zu verhindern, dass er sich bereits nach dem Entfernen der Steuerspannung von den Motoren über den erforderlichen Abstand hinaus bewegt. Zur vollen Zufriedenheit gibt es natürlich die wheelRotateAngRad () -Methode, die der wheelRotateAng () -Methode ähnelt, mit dem Unterschied, dass die Werte für Drehwinkel und Winkelgeschwindigkeiten im Bogenmaß und im Bogenmaß pro Sekunde als Parameter verwendet werden.

Der Algorithmus der wheelRotateAng () -Methode lautet wie folgt.

1. Zunächst wird die Übereinstimmung der Werte von speed [] und ang [] mit einigen Randbedingungen überprüft. Offensichtlich unterliegt das Fahrgestell physischen Einschränkungen sowohl hinsichtlich der maximalen Winkeldrehgeschwindigkeit der Räder als auch hinsichtlich der minimalen (minimalen Abfahrgeschwindigkeit). Außerdem dürfen die Winkel in Ang [] nicht kleiner sein als der minimale feste Drehwinkel, der durch die Genauigkeit der Encoder bestimmt wird.

2. Als nächstes wird die Drehrichtung jedes Rades berechnet. Offensichtlich durch das Produktzeichen ang [i] * speed [i] ;

3. Die "Rotationsdistanz" Dw [i] für jedes Rad wird berechnet - die Anzahl der Encoder-Abtastungen, die durchgeführt werden müssen, um sich um den angegebenen Ang [i] zu drehen.
Dieser Wert wird durch die Formel bestimmt:

Dw [i] = ang [i] * WHEEL_SEGMENTS / 360 ,
Dabei ist WHEEL_SEGMENTS die Anzahl der Segmente des Encoderrads (volle Umdrehung).

4. Der Spannungswert am Motortreiber wird aufgezeichnet.


5. Gemäß der Kalibrierungstabelle für jedes Rad wird der Anfangswert des PWM-Signals bestimmt, der die Drehung des Rads mit der erforderlichen Geschwindigkeitsgeschwindigkeit [i] sicherstellt. Welche Art von Kalibriertabelle und woher kam sie - wir werden sie weiter analysieren.

6. Die Drehung der Motoren wird gemäß den berechneten Werten für Geschwindigkeit und Drehrichtung gestartet. Im Text der Klassenimplementierung ist dafür die private Methode _wheel_rotate_sync () verantwortlich.

Wir gehen noch tiefer. Die _wheel_rotate_sync () -Methode arbeitet nach dem folgenden Algorithmus:

1. In einer Endlosschleife wird geprüft, ob für jedes Rad der Zähler der Geberantworten für die Umdrehungsentfernung Dw [i] erreicht wurde . Wenn einer der Zähler Dw [i] erreicht ist, stoppen alle Räder und verlassen den Zyklus und verlassen dann die Funktion (Schritt 5). Dies geschieht aus folgenden Gründen. Aufgrund der Genauigkeit der Messung des Drehwinkels ist es eine sehr häufige Situation, wenn der berechnete Abstand Dw [i] eines Rades erhalten wird, indem ein nicht ganzzahliger Wert auf eine kleinere Seite und Dw [j] des zweiten Rades auf eine größere Seite gerundet werden . Dies führt dazu, dass nach dem Anhalten eines der Räder das zweite Rad weiter dreht. Bei einem Fahrgestell mit Differentialantrieb (und vielen anderen) führt dies zu einer ungeplanten "Drehung" des Roboters am Ende der Aufgabe. Um die räumliche Bewegung des gesamten Fahrgestells zu organisieren, müssen daher alle Motoren gleichzeitig gestoppt werden.

2. Wird Dw [i] nicht erreicht, wird in der Schleife geprüft, ob der Encoder die nächste Operation ausführt (die Variable _syncloop [w] , die vom Encoder-Interrupt aktualisiert und in dieser Endlosschleife zurückgesetzt wird). Wenn die nächste Kreuzung auftritt, berechnet das Programm das Modul der aktuellen Winkelgeschwindigkeit jedes Rads (Grad / s) gemäß der offensichtlichen Formel:

W [i] = (360 · tau [i]) / WHEEL_SEGMENTS ,
wo:
tau [i] - der Durchschnittswert der Zeit zwischen den letzten beiden Antworten der Encoder. Die "Tiefe" des Mittelungsfilters wird durch MEAN_DEPTH bestimmt und ist standardmäßig 8.

3. Basierend auf den berechneten Raddrehzahlen werden die absoluten Fehler als Differenz zwischen der eingestellten und der tatsächlichen Winkelgeschwindigkeit berechnet.

4. Basierend auf den berechneten Fehlern wird die Steueraktion (PWM-Signalwert) für jeden Motor korrigiert.

5. Nach Erreichen von Dw [i] werden die Motoren im Falle eines aktiven Abschaltvorgangs mit Kurzzeitumkehrspannung beaufschlagt. Die Dauer dieses Effekts wird anhand der Kalibrierungstabelle (siehe unten) bestimmt und liegt normalerweise zwischen 15 und 40 ms.

6. Die Motoren werden vollständig entlastet und _wheel_rotate_sync () wird beendet .

Ich habe bereits zweimal eine bestimmte Kalibrierungstabelle erwähnt. In der Bibliothek befindet sich daher eine spezielle Wertetabelle, die im EEPROM des Roboterspeichers gespeichert ist und Datensätze mit drei verwandten Werten enthält:

1. Spannung an den Motorklemmen. Sie wird berechnet, indem der Wert des PWM-Signals in die tatsächliche Spannung umgewandelt wird. Hierzu wird in Schritt 4 der Methode wheelRotateAng () die tatsächliche Spannung am Motorfahrer erfasst.

2. Die Winkelgeschwindigkeit des Rades (ohne Last), die einer gegebenen Spannung entspricht.

3. Die Dauer des Hard-Stop-Signals, die dieser Winkelgeschwindigkeit entspricht.
Standardmäßig beträgt die Größe der Kalibrierungstabelle 10 Datensätze (bestimmt durch die Konstante WHEEL_TABLE_SIZE in der Datei config.h ) - 10 Dreifache der Werte „Spannung - Winkelgeschwindigkeit - Dauer des Stoppsignals“.

Um die Werte aus 2 und 3 Einträgen in dieser Tabelle zu ermitteln, wird eine spezielle Methode verwendet - wheelCalibrate (Byte Wheel) .

Schauen wir uns das etwas genauer an. Diese Methode implementiert eine Folge von Aktionen, um die fehlenden Werte in der Motor / Rad-Kalibrierungstabelle zu ermitteln sowie die minimale Startwinkelgeschwindigkeit und die maximale Radwinkelgeschwindigkeit zu ermitteln.

Zur Kalibrierung wird der Roboter auf einen Ständer montiert, alle Raddrehungen während der Kalibrierung werden ohne Last ausgeführt.

1. Zuerst müssen Sie die Mindeststartgeschwindigkeit bestimmen. Das geht ganz einfach. In einem Zyklus wird die Steuer-PWM ab 0 mit einem Inkrement von 1 dem Motor zugeführt. Bei jedem Schritt wartet das Programm eine gewisse Zeit, bestimmt durch die Konstante WHEEL_TIME_MAX (normale Verzögerung () ). Nach Ablauf der Wartezeit wird geprüft, ob der Start abgeschlossen ist (durch Ändern des Wertes des Geberzählers). Ist die Umlenkung beendet, wird die Winkelgeschwindigkeit des Rades berechnet. Zur Erhöhung der Sicherheit wird zum Wert der PWM, die dieser Startgeschwindigkeit entspricht, der Wert 10 addiert, wodurch sich das erste Wertepaar "Spannung am Motor" - "Winkelgeschwindigkeit" ergibt.

2. Nachdem die Startgeschwindigkeit ermittelt wurde, wird der PWM-Schritt berechnet, um die Kalibrierungstabelle gleichmäßig zu füllen.

3. Im Zyklus wird das Rad für jeden neuen PWM-Wert um 2 volle Umdrehungen gedreht und die Winkelgeschwindigkeit wird nach einem Algorithmus gemessen, der der _wheel_rotate_sync () -Methode ähnelt. Im gleichen Zyklus wird auch durch sukzessive Approximation der optimale Wert der Dauer des Hard-Stop-Signals gemessen. Zunächst wird offensichtlich großer Wert darauf gelegt. Und dann wird es im "Turn-Stop" -Modus getestet. Als Optimum wird der Maximalwert der Stoppsignaldauer gewählt, bei dem der eingestellte „Abbiegedistanz“ nicht überschritten wird. Mit anderen Worten, ein solcher Wert der Signaldauer, bei dessen Zufuhr zum Motor einerseits die Trägheit unterdrückt wird, und andererseits keine kurzfristige Rückwärtsbewegung vorliegt (die vom selben Geber festgelegt wird).

4. Nach Abschluss der Kalibrierung wird die Steuerspannung für den kalibrierten Motor nicht mehr angelegt und die Kalibrierungstabelle für dieses Rad wird im EEPROM aufgezeichnet.

Ich habe alle möglichen Kleinigkeiten der Umsetzung ausgelassen und versucht, das Wesentliche festzustellen. Möglicherweise stellen Sie fest, dass die Methoden wheelRotateAng () und wheelRotateAngRad () Funktionen blockieren. Dies ist der Preis für die Genauigkeit der Bewegung und eine relativ einfache Integration in Benutzerskizzen. Es wäre möglich, einen kleinen Task-Manager mit festem Timing zu erstellen, dies würde jedoch erfordern, dass der Benutzer seine Funktionalität streng in das zugewiesene Zeitkontingent einbettet.

Für eine nicht blockierende Anwendung verfügt die API über die Funktion wheelRotate (float * speed) . Es führt, wie aus der Parameterliste ersichtlich, einfach die Drehung der Räder mit den eingestellten Drehzahlen durch. Die Drehzahl wird in der Sync () -Methode des Robotergehäuses angepasst, die in der Sync () -Methode des gleichnamigen Miro-Klassenobjekts aufgerufen wird. Entsprechend den Anforderungen an die Struktur der Benutzerskizze sollte diese Methode bei jeder Iteration der Hauptschleife () der ARDUINO-Skizze aufgerufen werden.

In Schritt 4 habe ich in der Beschreibung der _wheel_rotate_sync () - Methode die "Steuerkorrektur" des Motors erwähnt. Wie haben Sie es erraten? Dies ist der PID-Regler. Nun, genauer gesagt PD-Controller. Wie Sie wissen (in der Tat - nicht immer), ist der beste Weg, die Koeffizienten des Reglers zu bestimmen, die Auswahl. In der Konfigurationsdatei config.h gibt es eine Definition:

 #define DEBUG_WHEEL_PID 

Wenn Sie es auskommentieren , wird beim Aufrufen der moveDist () -Methode der Miro-Klasse das folgende invertierte Diagramm des relativen Fehlers bei der Steuerung der Winkelgeschwindigkeit eines der Roboterräder (links) in der Roboterkonsole angezeigt.



Hat nichts ähnliches)? Nach unten ist die Zeit (jeder Balken ist ein Schritt des Regelzyklus) und der Fehlerwert wird rechts gespeichert (wobei das Vorzeichen erhalten bleibt). Hier sind zwei Paare von Graphen auf der gleichen Skala mit unterschiedlichen Koeffizienten des PD-Controllers. "Buckel" sind nur die "Wellen" des Überschießens. Die Zahlen auf den horizontalen Balken sind ein relativer Fehler (unter Beibehaltung des Vorzeichens). Einfache Visualisierung des Reglers zur manuellen Anpassung der Koeffizienten. Im Laufe der Zeit hoffe ich, ein automatisches Setup durchzuführen, aber fürs Erste.

Hier ist so ein Adok :-)

Schauen wir uns zum Schluss ein Beispiel an. Direkt aus der API_Miro_moveDist-Bibliothek:

 #include <Miro.h> using namespace miro; byte PWM_pins[2] = { 5, 6 }; byte DIR_pins[2] = { 4, 7 }; byte ENCODER_pins[2] = { 2, 3 }; Miro robot(PWM_pins, DIR_pins, ENCODER_pins); int laps = 0; void setup() { Serial.begin(115200); } void loop() { for (unsigned char i = 0; i < 4; i++) { robot.moveDist(robot.getOptLinSpeed(), 0, 1, true); delay(500); robot.rotateAng(0.5*robot.getOptAngSpeed(), -90, true); delay(500); } Serial.print("Laps: "); Serial.println(laps); laps++; } 

Aus dem Programmtext sollte alles klar sein. Wie es funktioniert - im Video.


600 x 600 mm Fliesen und 5 mm Fliesenabstände. Theoretisch sollte der Roboter ein Quadrat mit einer Seitenlänge von 1 Meter umrunden. Natürlich „schwebt“ die Flugbahn davon. Aus Gründen der Fairness ist es jedoch erwähnenswert, dass es in der Version des Roboters, die ich zu Testzwecken verlassen habe, ziemlich drehende Motoren gibt, die nur schwer langsam zu fahren sind. Aber bei hoher Geschwindigkeit und Schlupf gibt es einen Ort, an dem man sein kann, und Trägheit ist nicht einfach zu bewältigen. Motoren mit einer höheren Getriebeübersetzung (wie sie auch bei unseren MIRO-Robotern nur während des Tests nicht zur Verfügung standen) sollten sich etwas besser verhalten.

Wenn es unverständliche Momente gibt, kläre, diskutiere und verbessere ich diese gerne. Feedback ist generell interessant.