Ein Integralregler für die Geschwindigkeit

Mit den Motor-Encodern kann der Roboter nicht nur geradeaus fahren oder eine festgelegte Distanz zurücklegen, sondern er kann auch verwendet werden, die Geschwindigkeit des Roboters zu regeln.

Ohne Geschwindigkeitsregelung würde zum Beispiel ein fester PWM-Wert für den Motor bei abnehmender Batteriespannung zu einem langsamer fahrenden Roboter führen. Mit Regler kann jedoch der Motorwert so angepasst werden, dass der Roboter konstant mit der vorgegebenen Geschwindigkeit fährt.

Geschwindigkeit messen

Bevor ein Regler für die Geschwindigkeit vorgestellt wird, soll die aktuelle Geschwindigkeit des Roboters über die serielle Schnittstelle ausgegeben werden, um sie anschließend mit einem geeignetem Program als Liniendiagramm darzustellen zu können.

Zum Bestimmen der Geschwindigkeit des Roboters, müssen für ein festes Zeitintervall dZeit die vom linken und rechten Motor erzeugten Encoder-Impulse ermittelt und anschließend gemittelt werden:

int impulseLinks = encoders.getCountsAndResetRight();   // Impulse auslesen und zurücksetzen
int impulseRechts = encoders.getCountsAndResetLeft();

impulseRoboter = (impulseLinks + impulseRechts)/2;      // Mittelwert bestimmen

Die Geschwindigkeit des Roboters vRoboter wird dann in Encoderimpulse pro Sekunde angegeben.

int vRoboter = impulseRoboter/dZeit;  // Robotergeschwindigkeit in Impulsen pro Sekunde

Würde die Geschwindigkeit so oft wie möglich, also mit einem möglichst kleinem dZeit bestimmt werden, so würden die sich ergebenden Werte ziemlich stark schwanken. Diese Quantisierungsfehler entstehen, wenn pro Zeitintervall nur sehr wenige Encoderimpulse gemessen werden. Wird dann in einem Zeitintervall nur ein Impuls mehr oder weniger gezählt, führt das zu einer relativ großen Änderung der ermittelten Geschwindigkeit.

Beispiel

In einem Zeitintervall von 4 ms werden 9 bzw. 10 Impulse gezählt. Wird für beide Fälle die Geschwindigkeit bestimmt, so ergeben sich folgende Geschwindigkeiten:

 9  Impulse in 4 ms  ->   2250 Impulse pro Sekunde
10  Impulse in 4 ms  ->   2500 Impulse pro Sekunde

Das ergibt eine Abweichung von 10 %. Wird das Zeitintervall vergrößert, wird die prozentuale Abweichung immer kleiner, und die ermittelte Geschwindigkeit schwankt daher weniger. Das Zeitintervall, in welchem die Encoder-Impulse bestimmt werden, sollte so festgelegt sein, dass im Mittel ca. 50 Impulse detektiert werden.

Zeitverlauf der Geschwindigkeit

In der Abbildung ist der Verlauf der Geschwindigkeit des Zumo 32u4 dargestellt, wenn er aus dem Stand auf den Motorwert setSpeeds(300,300) beschleunigt. Es ist gut zu erkennen, dass die Geschwindigkeit relativ schnell stark ansteigt und dann ab ca. 160 ms die Maximalgeschwindigkeit erreicht.

Aufgaben

Beschreiben Sie weitere Situationen, in denen es sinnvoll ist die Motordrehzahl zu regeln.

Lassen Sie den Roboter über eine Zeit von ca. 0,5 Sekunden mit verschiedenen Geschwindigkeiten (200, 300, 400) fahren. Bestimmen sie gleichzeitig die aktuelle Geschwindigkeit in Impulsen pro Sekunde.
Geben Sie sowohl die Zeit als auch die Geschwindigkeit während des Beschleunigungsvorgangs auf dem seriellen Monitor aus.
Stellen Sie abschließend ihr Ergebnisse mit einem geeignetem Programm (z.B. Excel oder Openoffice) graphisch dar (y-Achse: Impulse pro Sekunde, x-Achse: Zeit in Millisekunden) und beantworten Sie folgende Fragen:

Wie lange benötigt der Roboter, bis er seine jeweilige Maximalgeschwindigkeit erreicht hat?
Welcher (mathematischer) Funktionstyp beschreibt den Zusammenhang zwischen Zeit und Geschwindigkeit am besten.

Hinweise für die Bearbeitung

Nutzen Sie für eine bessere Genauigkeit bei der Zeitmessung die micros() Funktion. Beachten Sie aber, das die Geschwindigkeit in Impulsen pro Sekunde und nicht in Impulsen pro Mikrosekunde angegeben werden soll.

Trennen sie beide Werte mit einem Tabulator (\t):

Serial.print(zeit), Serial.print("\t"),Serial.println(vRoboter);

Messen Sie die Geschwindigkeit in einem Zeitintervall, welches im Mittel ca. 50 Impulse detektiert (ca. 20 ms).

Verwenden Sie den Programmauszug:

/*   Programmauszug zur Geschwindigkeitsmessung    */

/*  ...  */

unsigned long dZeit    =   5 * 1e3;        // Zeitintervall für die Geschwindigkeitsmessung (5000 Mikrosekunden)
unsigned long fahrzeit = 500 * 1e3;        // Dauer der Messung (0,5 Sekunden)

unsigned long jetzt         = micros();
unsigned long startzeit     = jetzt;       // Startzeitpunkt der Messreihe
unsigned long letzteMessung = jetzt;       // Zeitpunkt der letzten durchgeführten Messung

jetzt = micros();                          // vergangene Zeit in Microsekunden
motors.setSpeeds(MOTORPOWER, MOTORPOWER);
while (jetzt - startzeit < fahrzeit) {     // Roboter soll `fahrzeit` fahren
  
  jetzt = micros();                        // vergangene Zeit in Microsekunden
  if (jetzt - letzteMessung >= dZeit) {    // Zeitintervall zur Geschwindigkeitsmessung
    letzteMessung = jetzt;
    int impulseRight = encoders.getCountsAndResetRight(); // Impulse auslesen und zurücksetzen
    int impulseLinks = encoders.getCountsAndResetLeft();
    
    /*  ...  */ 

  } 
} 
/*  ...  */

Vorbetrachtungen zum Regler

Aber wie würde nun ein Regler für die Geschwindigkeit aussehen? Vom Linienfolger ist bereits der Proportionalregler bekannt. Angenommen, der Roboter soll mit einer Geschwindigkeit von 2000 Encoder-Impulsen pro Sekunde fahren. Dann würde sich die Regelabweichung (also der Fehler) wie folgt berechnen:

fehler = 2000 - vRoboter     // Fehler: Abweichung der Geschwindigkeit vom Sollwert

Mit dem Proportionalregler würde dann das Motorsignal für die setSpeeds()-Funktion bestimmt werden:

motorSignal = kp * fehler           // P-Regler für das Motorsignal
setSpeeds(motorSignal,motorSignal)  // einstellen der Geschwindigkeit

Für den Fall, dass der Roboter sich mit der Sollgeschwindigkeit von 2000 Impulsen pro Sekunde bewegen würde, würde sich für den Fehler der Wert fehler = 0 ergeben. Dadurch ergibt sich auch für das Motorsignal der Wert motorSignal = 0. Der Roboter würde also stehen bleiben.

Ein Proportional-Regler, der das Motorsignal einfach nur proportional zum Fehler bestimmt, ist somit zur Regelung der Geschwindigkeit ungeeignet.

Ebenfalls ist der Differential-Regler ungeeignet, da er nur auf Änderungen des Fehlers eingeht. Steht der Roboter einfach nur still, so ändert sich der Fehler nicht und es würde sich für das Motorsignal nur der Wert 0 ergeben.

Integralregler

Ein weiterer, bisher nicht besprochener Regler ist der Integralregler. Der Integralregler (I-Regler) ändert die Stellgröße nicht proportional zum Fehler wie der Proportionalregler oder in Abhängigkeit von der Änderung des Fehlers wie der Differentialregler, sondern basierend auf der Summe (oder dem Integral) der vorhergehenden Fehler.

Aus mathematischer Sicht entspricht diese Summe der Fehler dem Integral der Fehler $e(t)$ über die Zeit $t$ .

\begin{aligned} \int_{0}^{t} e(t) \text{ d}t &\approx e(t_0)\ \cdot \Delta t + e(t_1)\ \cdot \Delta t + \cdots \\ &= ( e(t_0) + e(t_1) + \cdots )\ \Delta t \end{aligned}

Für den Pseudocode kann das Integral mit Hilfe der einzelnen Summen $e(t_i)\cdot \Delta t$ berechnet werden. Zu jedem neuen Zeitpunkt wird zu dem bereits berechneten Integral-Wert der neue Fehler $e(t_i)\cdot \Delta t$ hinzuaddiert. Dadurch wird aus dem komplizierten Integral eine sehr einfache Addition:

integral = integral + fehler * dTime // Aufsummieren der Fehler zum Integralanteil

Der Integralanteil wird zu einer einfachen Summe, wenn bei konstanten Zeitintervallen die Multiplikation mit dTime weggelassen wird:

integral = integral + fehler         // Aufaddieren des Fehlers liefert den Integralanteil

Zur Regelung der Motorgeschwindigkeit wird der Integral-Anteil, ähnlich wie der Proportional-Anteil, mit einem Faktor $k_i$ gewichtet. Damit ergibt sich der folgende Pseudocode für den Integralregler :

// Pseudocode eines Integralreglers für die Geschwindigkeit //
fehler      = 2000 - vRoboter        // Fehler: Abweichung der Geschwindigkeit vom Sollwert
integral    = integral + fehler      // Aufsummieren der Fehler
motorSignal = integral * ki          // Integralanteil mit Faktor ki wichten
setSpeeds(motorSignal,motorSignal)   // einstellen der Geschwindigkeit

Integralanteil beschränken - Der Windup-Effekt

Wenn bei mit einem Integralregler die Sollgröße, also hier die Geschwindigkeit, nicht oder nur sehr langsam erreicht werden kann, so führt das zu einem stark ansteigenden Integralanteil. Dieser Effekt wird Windup-Effket genannt und kann unter anderem zu einer verlangsamten Regelgeschwindigkeit des Reglers führen.

Aus diesem Grund sollte der Integralanteil immer auf sinnvolle Werte begrenzt werden. Die Wahl der Grenzen für den Integralanteil hängt dabei auch vom gewählten Faktor $k_i$ ab.

Für den Geschwindigkeitsregeler mit $k_i=0.1$ und maximalen Motorwerten von 400 ist die Begrenzung des Integralanteils auf -4000 ... +4000 sinnvoll:

integral = constrain(integral,-4000,4000);

Beispiel Geschwindigkeitsregelung

Ein Roboter soll mit einer Geschwindigkeit von 2000 Impulse pro Sekunde fahren. Hierfür wird ein Integralregler mit $k_i= 0.1$ verwendet. Der verwendete Regler hat keinen weiteren P-Anteil oder D-Anteil.

In der Abbildung ist der Verlauf der Werte für die Geschwindigkeit, den Fehler, den Integralanteil und das Motorsignal dargestellt. Die einzelnen Größen können in der Legende ein- oder ausgeblendet werden.

Um zu erklären, wie der Integralregler genau funktioniert, sind in der Tabelle die Werte für die ersten 5 Zeitpunkte des Reglers noch einmal aufgelistet.

#	Zeit [ms]	Geschwindigkeit	Fehler	Integralanteil	Motorsignal
1.	10	0	2000	2000	200
2.	20	349	1651	3651	365
3.	30	1150	850	4000	400
4.	40	1750	250	4000	400
5.	50	2250	-250	3750	375

Bei der 1. Messung ist die Geschwindigkeit des Roboters noch vRoboter = 0. Mit der angenommenen Sollgeschwindigkeit von 2000 ergibt sich ein Fehlerwert von fehler = 2000. Da der Integralanteil anfangs 0 war, ist nach der 1. Messung der Integralanteil integral = 2000 und für das Motorsignal ergibt sich mit $k_i=0.1$ der Wert motorSignal = 200. Der Roboter fängt an, sich langsam zu bewegen.

Bei der 2.Messung erkennt der Regler, dass der Roboter bereits eine Geschwindigkeit von vRoboter = 349 hat und ermittelt mit fehler = 1651 den neuen Integralanteil:

integral =  2000 + 1651 = 3651

Dies führt zu einem ansteigenden Motorwert von motorSignal = 365.

Bei der 3. Messung ermittelt der Regler bereits eine Geschwindigkeit von vRoboter = 1150 und damit einen Fehler von fehler = 850. Würde der Fehler einfach zu dem alten Integralanteil hinzuaddiert werden, würde sich ein Wert von integral = 4501 ergeben, was zu motorSignal = 450 führen würde. Da der Integralanteil aber auf einen Maximalwert von +4000 begrenzt sein soll, bestimmt der Regler nach der 3. Messung den Wert integral = 4000. (Bei dem Roboter Zumo 32u4 ist der maximale Motorwert 400)

Bei der darauffolgenden 4. Messung hat sich der Roboter der Sollgeschwindigkeit bereits bis auf fehler = 250 genähert. Da der Integralanteil begrenzt wird, bleibt dieser trotzdem auf dem Wert integral = 4000. Das Motorsignal bleibt ebenfalls gleich (motorSignal = 400).

Im nächsten Schritt, der 5. Messung, überschreitet der Roboter erstmals die Sollgeschwindigkeit (vRoboter = 2250) und der Fehler nimmt einen negativen Wert an fehler = -250. Durch den negativen Fehler wird nun auch der Integralanteil reduziert (integral = 3750) und das Motorsignal wird auf motorSignal = 375 verringert. Dadurch wird der Roboter wieder ein wenig langsamer.

In den nun folgenden Schritten pendelt die Geschwindigkeit noch etwas um den geforderten Sollwert herum, nähert sich diesem aber letztlich immer mehr an.

Aufgaben

Erläutern Sie, warum für den Geschwindkeitsregler eine Begrenzung des Integralanteils auf -4000 bis +4000 sinvoll ist. Wie würde sich der Regler in dem Beispiel verhalten, wenn der Integralanteil nicht beschränkt wäre?

Pseudocode

Vollständiger Pseudocode für den I-Regler zum Regeln der Geschwindigkeit

vSoll = 2000                            // Sollwert der Geschwindigkeit in Impulsen pro Sekunde
ki        = 0.1                         // Integral-Faktor
integral  = 0                           // Startwert für Integral-Anteil


wiederhole
    vRoboter     = bestimmeV()          // ermitteln der aktuellen Geschwindigkeit mit den Motorencodern
    fehler       = vSoll - vRoboter     // Fehlerwert, Regeldifferenz
    integral     = integral + fehler    // Integral-Anteil 
    integral     = begrenze(integral)   // Integralanteil begrenzen                                                   
    motorSignal  = integral * ki        // Motorsignal bestimmen
                              
    setMotors(motorSignal,motorSignal)  // Aktualisieren der Motordrehzahlen

Aufgaben

Untersuchen Sie, welchen Einfluss die Begrenzung des Integralanteils auf das Verhalten eines Integralreglers für die Geschwindigkeit hat. Schreiben Sie dazu einen Integral-Regler, welcher die Geschwindigkeit des Roboters regelt.
Geben Sie den Verlauf der Geschwindigkeit, des Fehler, des Integral-Anteil und des Motorsignal dem seriellen Monitor aus. Stellen Sie die Werte mit einem geeigneten Programm als Liniendiagramm dar.
Analysieren Sie das Verhalten mit und ohne Begrenzung des Integralanteils und verschiedenen Werten für $k_i$ . Welchen Einfluss hat der Wert $k_i$ auf das Verhalten des Reglers. Welcher Wert führt zu einem Regelverhalten, welches mit wenigen Oszillationen schnell zum erreichen der Sollgeschwindigkeit führt.
Erweitern Sie die Geschwindigkeitsregelung (mit Begrenzung des Integralanteils) aus der letzten Aufgabe um folgende Eigenschaften:
1. Die Sollgeschwindigkeit soll mit den Tastern A und B eingestellt werden können.
2. Auf dem Display wird der aktuelle Wert für das Motorsignal (Wert für die Funktion setSpeeds()) angezeigt.
3. Der Roboter soll erkennen, wenn er gegen ein Hindernis fährt und es mit der roten LED anzeigen.
4. Der Roboter folgt einer Linie mit der vorgegeben Geschwindigkeit.

Geschwindigkeit messen​

Beispiel​

Zeitverlauf der Geschwindigkeit​

Vorbetrachtungen zum Regler​

Integralregler​

Integralanteil beschränken - Der Windup-Effekt​

Beispiel Geschwindigkeitsregelung​

Pseudocode​

Geschwindigkeit messen

Beispiel

Zeitverlauf der Geschwindigkeit

Vorbetrachtungen zum Regler

Integralregler

Integralanteil beschränken - Der Windup-Effekt

Beispiel Geschwindigkeitsregelung

Pseudocode