Odometrie

Unter Odometrie versteht man die Bestimmung der Position und Orientierung unter Zuhilfenahme des Antriebes. Man nutzt also das Wissen über den Radabstand, Radumfänge und Motorumdrehungen um zu berechnen, um wie viel Grad sich der Roboter seit der letzten Bewegungsänderung gedreht hat und welche Strecke dabei gefahren wurde. Ihr könnt hier gern euer mathematisches Wissen anwenden und euch ein eigenes Modell erarbeiten.

Grundkonzept und Annahmen

Während der PID-Controller die Steuerung der Motoren übernimmt, kann man die Änderungen der Motorpositionen auslesen. Die gefahrene Gesamtstrecke wird damit in Intervalle zerlegt, für die jeweils die Motorpositionen bekannt sind. Berechnet man nun die Änderung des Blickwinkels sowie die gefahrenen Strecken der einzelnen Intervalle und summiert diese korrekt, kann man seine Position ziemlich präzise bestimmen. Sind die Intervalle hinreichend klein, so lässt sich die Bewegung des Roboters auf einem Intervall als Kreisbogen approximieren. Unter dieser Annahme kann man die Winkel und Stecken berechnen.
Mit anderen Ansätzen und Annahmen lassen sich andere Modelle erzeugen, die ebenso ein Ergebnis liefern. Die Wahl des Verfahrens welches ihr verwenden wollt ist euch überlassen.

Da die Berechnung während des Fahrens unnötig Leistung verbrauchen würde, ist es sinnvoller, während des Fahrens die Motorpositionen für jedes Intervall zu speichern und diese Liste auszuwerten, wenn der Roboter einen Punkt erreicht hat.

Konstanten

Damit die Odometrie reibungslos funktioniert, müssen zwei Werte gemessen werden. Zum einen braucht man die distance_per_tick, eine Konstante, mit der man Motorpositionen in zurückgelegte Entfernung umrechnen kann, zum anderen braucht man den Radabstand $a$ . Es erwies sich in der Vergangenheit als sinnvoller, die distance_per_tick zuerst zu messen, da sich der Wert nie verändern sollte, und danach den Radabstand zu kalibrieren. Sollten die Werte einen leichten Fehler enthalten, muss im Normalfall der Radabstand neu kalibriert werden.

Mathematische Vorbetrachtungen

Die Nomenklatur mag dem Leser an dieser Stelle etwas seltsam erscheinen, ergibt jedoch im Kontext der Herleitung später Sinn.

Lemma 1:

Lemma1

Für zwei Kreisbögen $d_l$ und $d_r$ mit dem Radius $r_l$ bzw. $r_r$ über einem Winkel $\alpha$ gilt:
$\large{\alpha = \frac{d_l}{r_l} = \frac{d_r}{r_r}}$ oder auch $\large{d_l = \alpha \cdot r_l}$ , $\large{d_r = \alpha \cdot r_r}$

Für einen Kreisbogen $d_w$ mit dem Radius $r_w = r_r + r_l$ über $\alpha$ gilt:
$\large{\alpha = \frac{d_w}{r_w} = \frac{d_w}{r_r + r_l}}$ oder auch $\large{d_w = \alpha \cdot (r_r + r_l)}$

Daraus folgt:
$\large{d_r + d_l = \alpha \cdot r_r + \alpha \cdot r_l = \alpha \cdot (r_r + r_l) = d_w}$

$\large{\alpha = \frac{d_w}{r_w} = \frac{d_r + d_l}{r_r + r_l}}$

Analog gilt für einen Kreisbogen $d_a$ mit $r_a = r_r - r_l$ :
$\large{d_r - d_l = d_a}$

$\large{\alpha = \frac{d_a}{r_a} = \frac{d_r - d_l}{r_r - r_l}}$

Lemma 2:

Sei $r_s$ der Durchschnitt aus $r_r$ und $r_l$ . Sei $d_s$ der Kreisbogen mit Radius $r_s$ über $\alpha$ . Es gilt:
$\large{r_s = \frac{(r_r + r_l)}{2}}$

$\large{d_s = \alpha \cdot r_s = \alpha \cdot \frac{(r_r + r_l)}{2} = \frac{(\alpha \cdot r_r + \alpha \cdot r_l)}{2}} = \frac{(d_r + d_l)}{2}$

Lemma 3:

Die Sehne $s$ unter einem Kreisbogen $d_s$ mit dem Radius $r_s$ über $\alpha$ hat die Länge:
$\large{s = 2 \cdot r_s \cdot sin(\frac{\alpha}{2})}$

Falls das nicht selbstverständlich für euch ist, sei an dieser Stelle die Empfehlung ausgesprochen, diese einmal kurz mit minimaler Trigonometrie herzuleiten. Für einen Beweisansatz, sowie einen Verweis auf die historische Bedeutung: Sehne_(Geometrie)

Distanz und Richtungsänderung

odometry_derivation

Bekannt ist der Radabstand $a$ , sowie die vom linken bzw. rechten Rad im betrachtetem Intervall zurück gelegten Strecken $d_l$ und $d_r$ . Gesucht ist die zurück gelegte Strecke $s$ unter dem gefahrenen Kreisbogen $d_s$ , sowie die Änderung des Blickwinkels $\alpha$ . \

Nachdem ihr die in Lemma 1 bis 3 erklärten Konzepte verstanden habt, ist die Herleitung recht einfach. Ihr könnt hier eure mathematischen Fähigkeiten testen und erst einmal selber versuchen die Herleitung zu finden.

Lösung:

Man beobachte:
$\large{a = r_r - r_l}$

Mit Lemma 1 folgt:
$\large{\alpha = \frac{d_a}{r_a} = \frac{d_r - d_l}{r_r - r_l}} = \frac{d_r - d_l}{a}$

Falls $\large{d_l > d_r}$ , so hat der Roboter eine Rechtsdrehung durchgeführt und $\alpha$ wird automatisch negativ.

’''

$r_s$ ist der Durchschnitt aus $r_r$ und $r_l$ mit dem dazugehörigen Kreisbogen $d_s$ über $s$ . Aus Lemma 2 folgt direkt:
$\large{d_s = \frac{(d_r + d_l)}{2}}$

’''

$\large{\alpha = \frac{d_s}{r_s}}$ bzw. $\large{r_s = \frac{d_s}{\alpha}}$
Mit Lemma 3 folgt:
$\large{s = 2 \cdot r_s \cdot sin(\frac{\alpha}{2}) = 2 \cdot \frac{d_s}{\alpha} \cdot sin(\frac{\alpha}{2}) = a \cdot \frac{d_r + d_l}{d_r - d_l} \cdot sin(\frac{d_r - d_l}{2a})}$

Man beachte, der Fall $d_r = d_l$ bzw. $\alpha = 0$ muss extra behandelt werden. Dies ist jedoch trivial und euch überlassen.

Abbildung ins Koordinatensystem

Kennt man nun $\alpha$ , $s$ , sowie die Ausgangs-Blickrichtung $\gamma$ für alle Intervalle, so ist der nächste Schritt daraus die Positionsänderung $(\varDelta X, \varDelta Y)$ und die Änderung der Blickrichtung $\varDelta \gamma$ auszurechnen. Dies entspricht der Berechnung eines gerichteten Vektors für jedes Intervall. Die Summe der Vektoren ergibt die Positionsänderung auf der gesamten Strecke. Da am Ende nur die Gesamtänderung relevant ist, kann man auch gleich die Summe bilden, während man durch alle Intervalle durchgeht.

odometry_derivation_explicit_triangle

Dabei ist zu beachten, dass der Winkel $\alpha$ mathematisch positiven Drehsinn verwendet, die Himmelsrichtungen des Mutterschiffs jedoch nicht. Des Weiteren messt ihr die Distanzen zur Kalibrierung vermutlich in cm wohingegen das Mutterschiff Koordinaten als Einheit verwendet. An welcher Stelle ihr von einem System ins andere umrechnet, ist euch überlassen.

Hinweise

Wenn man beachtet, dass die Punkte auf der Karte im Raster immer abwechselnd rot und blau sind, kann man die Koordinaten genauer runden.
Da alle Formeln mit Winkeln im Bogenmaß arbeiten, empfiehlt es sich, Winkel im gesamten Programm im Bogenmaß zu speichern und sie nur zur Ausgabe ins Gradmaß umzurechnen.
Die Formeln funktionieren auch für Rechtskurven. Es muss nichts umgestellt, werden. Der Winkel $\alpha$ wird automatisch negativ.
Für die Bestimmung des Winkels $\alpha$ kann man auch versuchen, den Gyro-Sensor zu verwenden. Allerdings ist dieser Sensor nicht immer präzise (in Verbindung mit dem EV3), was zu größeren Abweichungen führen kann als die Berechnung über die Motorpositionen.