Ihrem Roboter die Mobilitätssteuerung eines echten Mars Rover geben: Teil 2

In Teil 1 ging es um die Theorie hinter der PID-Steuerung – einer weit verbreiteten Methode, die sicherstellt, dass die mechanische Ausgabe eines Betätigungselements (Geschwindigkeit, Position usw.) mit dem vom Steuerungsprogramm vorgegebenen Wert übereinstimmt. Ich habe gezeigt, wie die komplexe Theorie in eine relativ einfache Arithmetik übersetzt wird, die auf einem einfachen Mikrocontroller ausgeführt werden kann. Sehen wir uns nun einen realen Code an, der für einen durch Servomotoren mit kontinuierlicher Rotation angetriebenen zweirädrigen Buggy-Roboter geschrieben wurde. Ich habe einen Digital Signal Controller dsPIC33 mit 16 Bit von Microchip verwendet (einen PIC24-Mikrocontroller mit DSP-Erweiterungen), doch die meisten praktischen Probleme gelten auch für Projekte mit anderen Mikrocontrollern.

Erfassung der Raddrehungsdaten

Das Gerät zum Messen der Raddrehzahl besteht aus zwei Teilen: dem Rad selbst mit 32 radialen Schlitzen und einem Modul mit IR-Strahl und Reflektor, das am Chassis befestigt und an den Schlitzen (Abb. 1) ausgerichtet wird. Das Funktionsprinzip ist einfach.

Das Sensormodul besteht aus einer Infrarot-LED (IR-LED) und einer Fotodiode, die nebeneinander angeordnet sind und in Richtung der Schlitze zeigen. Wenn sich das Rad dreht, fällt der Strahl durch einen Schlitz und wird somit nicht von der Fotodiode wahrgenommen. Zwischen den Schlitzen wird er dann aber wieder zur Diode hin reflektiert. Das Ergebnis ist eine Impulsfolge, deren Wiederholfrequenz proportional zur Drehgeschwindigkeit des Rades ist. Wir benötigen eine Möglichkeit zur Messung dieser Drehfrequenz und glücklicherweise sind die meisten modernen Mikrocontroller zu diesem Zweck mit der integrierten Hardware „Input Capture“ ausgestattet (Abb. 2).

Im Prinzip könnte man die Geschwindigkeit mit Hilfe eines direkt vom Sensor getakteten Zählers messen. Das Auslesen und anschließende Löschen des Zählers in festgelegten Intervallen würde einen Wert für die Geschwindigkeit ergeben. Das Problem besteht darin, dass das Ausleseintervall bei langsamer Geschwindigkeit zu lang dauern müsste, um eine aussagekräftige Anzahl zu erfassen. Dies kann durch eine umgekehrte Herangehensweise umgangen werden: Der Zähler wird mit einem schnellen inkrementellen Zeitmesser ausgestattet und der Wert mithilfe der Sensorimpulse erfasst bzw. „aufgezeichnet“. Auf diese Weise erhalten wir stattdessen einen Messwert für den Impulszeitraum. Das „Input Capture (IC)“-Gerät des dsPIC enthält die gesamte benötigte Hardware und kann mit nur wenigen Zeilen Assembler-Code in Betrieb genommen werden. Leider gibt es eine Komplikation – auf dem von mir verwendeten Chip stehen zwei voneinander unabhängige IC-Geräte zur Verfügung, aber einer der Zeitgeber wird für eine andere Funktion benötigt. Es gibt eine Lösung: Der verbleibende Zeitgeber kann mithilfe einer anderen Hardware im IC-Gerät gemeinsam genutzt werden. Die beiden IC-Geräte werden in diesem Fall beide so eingerichtet, dass sie Zeitgeber 3 verwenden. Dieser zählt kontinuierlich von 0000 bis FFFF, wird auf 0000 zurückgesetzt und beginnt von vorne. Der Nachteil besteht darin, dass der Zähler nicht am Anfang jedes Ausleseintervalls neu gestartet werden kann, da die beiden Geräte zu unterschiedlichen Zeitpunkten (Abb. 3) ausgelesen werden.

Die rampenförmige Signalform stellt den vom Zeitgeber gezählten Wert von 0000 bis FFFF mit anschließender Wiederholung dar. Darunter wird die impulsartige Signalform eines Drehungssensors gezeigt, der den Zählerstand des Zeitgebers auf jeder ansteigenden Kante erfasst. Die Hardware „Input Capture“ speichert diesen „Schnappschuss“ im FIFO-Puffer und sendet gleichzeitig einen Interrupt an den Prozessor.

Geschwindigkeitsberechnung

Jetzt nimmt der Interrupt Service Code einen von einem früheren Interrupt gespeicherten Zählwert, subtrahiert ihn vom neuen Wert und erhält eine Zahl, die für die Sensorimpulsperiode steht. Im Diagramm werden die Zählwerte für die Schnappschüsse C1 und C2 zu den Zeitpunkten T1 und T2 ausgelesen und C2 - C1 entspricht dem Intervall T2 - T1. Ganz einfach. Es gibt jedoch zwei Probleme, die auftreten, wenn das Rad sich zu drehen beginnt:

• Beim ersten Interrupt gibt es keinen gültigen „vorherigen Zählwert“, sodass die erste Geschwindigkeitsberechnung einen falschen Wert ergibt.
• Es gibt eine Mindestgeschwindigkeit, da die Sensorimpulsperiode nicht länger dauern darf als die Zählung des Zeitgebers von 0000 bis FFFF. Es gibt außerdem eine Höchstgeschwindigkeit, dies dürfte aber wahrscheinlich kein Problem darstellen!

Beides bedeutet, dass die ersten Geschwindigkeitswerte, nachdem der Roboter beginnt, sich zu bewegen, wahrscheinlich falsch sind. Dagegen kann nicht viel getan werden, außer dass der PID-Steuercode die ersten Werte ignoriert, indem der Motor gestartet wird und das System ab dem Ruhezustand für eine geschätzte Zeitdauer im „Open-Loop“-Betrieb läuft, bis die Sensorwerte sich im richtigen Bereich einpendeln. In Bezug auf Abb. 3 gibt es ein weiteres Problem, das aber behoben werden kann. Denken Sie an die Geschwindigkeit, die zum Zeitpunkt T3 berechnet wird. Das Intervall von T2 bis T3 umfasst den Zählerüberlauf, sodass C3 - C2 die falsche Antwort liefert. Glücklicherweise erkennt die Software diese Situation, da C3 weniger als C2 ist, und wendet einen Korrekturfaktor an. Normalerweise ist der neue Wert größer als der vorherige, so wie bei C1 und C2.

Tatsächlicher Code

Die grundlegende Interrupt Service Routine für meinen dsPIC-Prozessor, bei der die Geschwindigkeit und die drei „rohen“ P-, I- und D-Werte ohne Anpassung und K-Faktoren berechnet werden, wird unten in Programmausdruck 1 angegeben.

Dies ist die Interrupt Routine für einen der beiden in FORTHdsPIC (meine eigene eingebettete Programmiersprache) eingebauten Kanäle für Drehungssensoren. Obwohl er im Assembler-Code des dsPIC („Bare-Metal“) geschrieben ist, sollte es einfach sein, zu sehen, wie sich die ursprüngliche komplexe PID-Gleichung in nur wenigen Subtraktions- und Additionsvorgängen ausdrücken lässt. Dieser Code wird bei jedem Sensorimpuls ausgeführt, wobei die rohen PID-Werte in zur Verarbeitung durch das Hauptprogramm bereiten Variablen gespeichert werden. Dieses Hauptprogramm, das mithilfe der Variablen den Motorsteuerungswert berechnet, wird in Teil 3 dieser Serie behandelt. Die Interrupt Routine enthält darüber hinaus eine einzelne Codezeile, die eine wichtige Navigationsaufgabe übernimmt – sie sorgt für Odometrie.

Odometrie

Der Raddrehungssensor liefert nicht nur Geschwindigkeitsdaten (Tachometrie): Durch die Zählung der Impulse wird eine Messung der Wegstrecke (Odometrie) durchgeführt. Das ist die Zeile „inc  ODOMTR“ in Programmausdruck 1. Jeder, der schon einmal Auto oder Motorrad gefahren ist, kennt einen Kilometerzähler: Es handelt sich um die digitale Anzeige der zurückgelegten Kilometer auf dem Armaturenbrett. Bei einem Roboter kann sie eine entscheidende Rolle für das Mobilitätssystem spielen. In diesem Fall wird die ODOMTR-Variable bei jedem Sensor-Interrupt erhöht, sodass jeder Schritt einer Raddrehung von einem Schlitz zum nächsten entspricht und insgesamt 32 Schritte eine vollständige Umdrehung ergeben. Um die zurückgelegte Strecke für die Zeit von einem Schlitz zum nächsten zu ermitteln, teilen Sie einfach den Radumfang πD durch 32, wobei D der Raddurchmesser ist. Die Räder dieser Encoder von Parallax haben einen Durchmesser von 66 mm, sodass die odometrische Auflösung 6,4 mm beträgt. Mein Roboter wird per Differenzialgeschwindigkeit gesteuert und benötigt Geschwindigkeitsdaten von jedem Rad zur Geradeausbewegung und Daten über die zurückgelegte Wegstrecke für präzise Wendemanöver. Mehr hierzu in Teil 3.

Quadrature Encoder

Der oben beschriebene einfache Drehungssensor hat eine ziemlich offensichtliche Einschränkung: Er kann nicht erkennen, in welche Richtung sich das Rad dreht. Durch Hinzufügen eines weiteren Fotosensors kann die Richtung der Drehungsdaten extrahiert werden. Der zweite Sensor ist so ausgerichtet, dass der Ausgangsimpuls seiner Signalform gegenüber dem ersten um genau 90° phasenversetzt ist (Abb. 4).

Ein einfaches D-Flipflop ist alles, was zur Decodierung dieser Signale notwendig ist. Die meisten neueren, für die Motorsteuerung entwickelten Mikrocontroller verfügen über zwei QE-Kanaleingänge mit der erforderlichen Hardware auf dem Chip. Mein Roboter benötigt keinen „Rückwärtsgang“, sodass ein einfacher Tachometer ausreicht, aber für andere Planeten vorgesehene Fahrzeuge müssen auch rückwärts fahren können.

Das nächste Mal inTeil 3

Ich werde den Teil des PID-Algorithmus behandeln, der den Motorantrieb steuert, und erklären, wie der Roboter „Koppelnavigation“ einsetzt. Zu guter Letzt kommen ein paar praktische Probleme zur Sprache – was passiert auf unebenem Gelände, wenn die Motorenbelastung schwankt? Erfahren Sie, wie ein Entwicklungsfehler im Mobilitätssteuerungssystem des Mars Curiosity Rover zu einem Radschaden führte. Sogar Weltraumforscher machen Fehler!

Wenn Sie mögen, folgen Sie mir auf Twitter. Ich verweise auf interessante Artikel über neue Elektronik und verwandte Technologien, und leite von mir entdeckte Tweets über Roboter, Weltraumforschung und andere Themen weiter.