Thymio folgt einer schwarzen Kante
Thymio folgt einer Bahn mit bloss einem Bodensensor
Die Fahrt auf einer schwarzen Linie gehört zum Grundrepertoire von Thymio. Dazu werden beide Bodensensoren benötigt. Einige Anwendungen1 benutzen aber einen Bodensensor für das Lesen von Informationen neben der Bahn, so dass für die Verfolgung der Linie nur ein Bodensensor übrig bleibt.
Die aufwändige und robuste Lösung
Die beiden Projekte Lichtmalerei und Strassennetzwerk verwenden als Weg einen 3 cm breite Linie, mit einem Gradienten von 'weiss' zu 'schwarz', welche quer zu Fahrtrichung liegt. Thymio wird nun so gesteuert, dass der Bodensensor ein mittleres Grau sieht. Die Richtungssteuerung geht so:
- Wenn die Messung den mittleren Wert ergibt, ist Thymio in der Mitte und muss geradeaus fahren.
- Wenn die Messung einen dunkleren Wert ergibt, ist Thymio zu weit rechts und muss nach links korrigieren.
- Wenn die Messung einen helleren Wert ergibt, ist Thymio zu weit links und muss nach rechts korrigieren.
Diese Methode funktioniert sehr gut, aber die Herstellung (Design und Druck) dieser Bahnen ist schwierig und aufwändig.
Es funktioniert auch mit einer einfachen schwarzen Linie!
Es wäre praktisch, wenn das 'einäugige' Verfolgen auch mit einer einfachen schwarzen Linie funktionieren würde. Das Projekt Thymio macht Musik zeigt, dass das tatsächlich möglich ist.
Es funktioniert aus folgendem Grund: Der Bodensensor misst nicht die Helligkeit eines Punktes, sondern er mittelt die Helligkeit auf einem Feld von etwa 6mm mal 6mm. Wenn die Kante genau unter dem Sensor ist, dann ist die eine Hälfte schwarz und die andere Hälfte weiss: Der Sensor sieht ein mittleres Grau. Wird der Sensor nach 'aussen' verschoben, wird der schwarze Anteil immer kleiner und der Sensor interpretiert dies als immer helleres grau. Wird der Sensor nach 'innen' verschoben, wird der schwarze Anteil immer grösser und der Sensor interpretiert dies als immer dunkleres grau.
Die obenstehende Grafik zeigt die Messwerte eines Bodensensors an einer schwarzen Kante: Auf der horizontalen Achse ist die Position des Sensors bezüglich der Kante aufgetragen. Der Wert 0mm bedeutet der Sensor ist genau über der Kante. Bei -5mm ist der Sensor 5mm weit von der Kante entfernt, und bei +5mm ist der Sensor 5mm weit über der schwarzen Linie.
Eine schwarzweisse Kante wird vom Sensor also als Gradient interpretiert. Allerdings ist die Breite des Gradienten , im Gegensatz zum echten Gradienten mit einer Breite von 30mm, bloss etwa 6mm.
Ein VPL Programm zum Verfolgen einer Linie mit einem Bodensensor
Mit diesem VPL program kann Thymio einer Linie mit einem Bodensesor folgen. Anders als bei den nächsten beiden Programmen müssen die Ansprechwerte in den Blöcken sorgfältig von Hand eingestellt werden.
Ein Blockly Programm zum Verfolgen einer Linie mit einem Bodensensor
Mit diesem Blockly program kann Thymio einer Linie mit einem Bodensensor folgen. Obwohl das Programm sehr kurz ist, funktioniert es fast so gut wie die Text-Version weiter unten.
Ein genauerer, adaptiver Algorithmus zum Verfolgen einer Linie mit einem Bodensensor
Unser Ziel ist es nun, den Algorithmus zu optimieren, so dass er auch bei einer scharfen Kante zuverlässig funktioniert.
Bei allen erwähnten Projekten wurde folgender Algorithmus (hier leicht abgewandelt) verwendet:
onevent prox
p1=prox.ground.delta[1] # der aktuelle Messwert wird nach p1 kopiert
devi = p1-525 # devi(ation) ist die Abweichung von p1 zum Mittelwert 525
if abs(devi)<170 then # wenn die Abweichung kleiner 170 sind wir auf dem Gradienten
motor.left.target=speed+devi/8 # links: devi/8 wird zur Geschwindigkeit speed addiert
motor.right.target=speed-devi/8 # rechts: devi/8 wird von der Geschwindigkeit speed subtrahiert
else # sonst haben wir den Gradienten verloren
motor.left.target=devi/4 # und drehen an Ort
motor.right.target=-devi/4
end
Die Werte 525 für den Mittelwert und 170 für Grenze des Gradienten wurden (wahrscheinlich) empirisch an den verwendeten Gradienten angepasst. Um den Prozess besser zu gestalten sollen nun diese Werte automatisch angepasst werden.
Dazu werden zuerst einmal die Konstante für den Mittelwert durch die Variable mean, und die Konstante für die maximale Abweichung durch vari(anz) ersetzt.
Die Variable mini beinhaltet den kleinsten und die Variable maxi den grössten je gemessenen Messwert p1. Die Grösse vari wird nun zu
$$vari=0.45 (maxi-mini)$$
berechnet. Die Grösse mean wird als
$$mean=\frac{maxi+mini}{2}$$
berechnet. Die entsprechenden Codezeilen sind im folgenden Listing eingefügt und werden bei jedem Prox-Event durchgeführt:
onevent prox
p1=prox.ground.delta[1]
call math.max(maxi,maxi,p1) #finde grösstes p1 --> maxi
call math.min(mini,mini,p1) #finde kleinstes p1 --> mini
call math.muldiv(vari,45,maxi-mini,100)
mean=(mini+maxi)/2
devi = p1-mean
if abs(devi)<vari then
motor.left.target=speed+devi/8
motor.right.target=speed-devi/8
else
motor.left.target=devi/4
motor.right.target=-devi/4
end
Leider ist der Wert devi, welche zur Steuerung verwendet wird, von verschiedenen äusseren Faktoren (wie Raumhelligkeit, Sensor, Art des Untergrunds etc.) abhängig. Daher wird die Abweichung devi mit der Konstanten SIGMA2 multipliziert und im Verhältnis zur Grösse vari normiert. :
$$ndev=\frac{SIGMA \cdot devi}{vari}=\frac{SIGMA\cdot (p1 - mean)}{vari}$$
Nachdem die Berechnung der Grössen mean und vari in das Unterprogramm statistics ausgelagert wurde, ergibt sich folgender Code:
onevent prox
p1=prox.ground.delta[1]
callsub statistics
call math.muldiv(ndev,SIGMA,p1-mean,vari)
if abs(ndev)<SIGMA then
motor.left.target=speed+60*ndev/100 # ....=speed+0.6*ndev
motor.right.target=speed-60*ndev/100 # ....=speed-0.6*ndev
else
motor.left.target=1*ndev/2
motor.right.target=-1*ndev/2
end
Der Proportional-Integral-Regler
Die Steuergrösse für die Regelung der Motoren ist proportional zur Abweichung vom Mittelwert. Diese Art der Regelung wird daher P-Regler genannt. Der Proportionalitätfaktor oder P-Faktor (hier 0.6) muss empirisch ermittelt werden: Wenn Thymio bei der Fahrt unruhig 'hin und her wackelt', dann ist der Wert zu gross. Wenn Thymio 'aus den Kurven fällt', dann ist der Wert zu klein.
Es gibt eine empirische Methode, den Schwingtest von Ziegler und Nichols, um die Grössenordnung des P-Faktors eines P-Reglers3 zu bestimmen: Man beginne mit einem sehr kleinen Faktor und vergrössere ihn kleinen Schritten, bis Thymio 'stabil' osziliert. Die Hälfte dieses gefundenen Wertes ist ein guter Startpunkt für weitere Versuche.
Diese Version des Programms funktioniert recht gut. Es gibt aber ein kleines Problem: Wenn der P-Faktor so gewählt ist, dass Thymio 'wackelfrei' auf geraden Wegstücken fährt, dann 'erwischt' er enge Kurven nicht. Wenn der P-Faktor so eingestellt ist, dass er enge Kurven fährt, dann wackelt er auf geraden Wegstücken.
Abhilfe bringt der sogenannte PI-Regler! Bei einem PI-Regler wird eine weitere Grösse, nämlich die aufkummulierte Summe der Abweichungen, berechnet und als zusätzliche Steuergrösse benutzt. Dazu wird eine weitere Variable ireg verwendet, in welcher alle Abweichungen ndev aufsummiert werden:
preg=60*ndev/100 #preg=0.6*ndev
ireg=ireg+10*preg/30 #ireg=ireg+0.33*preg
Der sogenannte I-Faktor (hier 0.33) wurde wieder empirisch ermittelt. Auch hier gibt der Ziegler-Nichols-Test einen Schätzwert, nämlich 1/Periodenlänge4 der stabilen Schwingung, welche beim Schwingtest ermittelt wurde.
Das Programm am Stück
#onevent startup
mini=1023
maxi=0
mean=512
vari=512
ireg=0
speed=100 #oder eben ein anderer Wert
onvent prox
p1=prox.ground.delta[1]
callsub statistics
call math.muldiv(ndev,SIGMA,p1-mean,vari)
if abs(ndev)<SIGMA then
preg=60*ndev/100
ireg=ireg+10*preg/30
motor.left.target=speed+(preg+ireg)
motor.right.target=speed-(preg+ireg)
else
ireg=0
motor.left.target=1*ndev/2
motor.right.target=-1*ndev/2
end
sub statistics
call math.max(maxi,maxi,p1)
call math.min(mini,mini,p1)
if maxi-mini>400 then
call math.muldiv(vari,45,maxi-mini,100)
mean=(mini+maxi)/2
end
Ein vollständiger Code ist hier. Happy Coding…