private:
WorldPosition pos;
In dieser Struktur befindet sich die aktuelle Odometrie-Position. Die Position wird repräsentiert durch die x- und y-Koordinate (angegeben in mm) und die Orientierung des Rollstuhls (angegeben im Bogenmaß: 0 = Norden, +Pi/2 = Osten, -Pi/2 = Westen, und Pi = Süden). Der Bezugspunkt der Position auf dem Rollstuhl ist die Mitte der Vorderachse.
boolean posChanged;
Die boolsche Variable posChanged zeigt an, ob sich der Rollstuhl seit der letzten Abfrage dieser Variablen bewegt hat. Ist dies der Fall, beinhaltet posChanged den Wert true. Hat sich dagegen die Position nicht geändert, ist posChanged gleich false.
public:
-
Odo(); und ~Odo();Konstruktor und Destruktor der Odometrie-Klasse: Derzeit sind weder Funktionalitäten für den Konstruktor noch für den Destruktor vorgesehen.
-
void initPosition(WorldPosition p);Mit dieser Funktion kann eine beliebige Rollstuhlposition vorgegeben werden. Eine Positions-Aktualisierung durch updatePosition erfolgt immer relativ zu der hier initiierten Position. Überdies wird sichergestellt, daß die Odometrie-Zähler auf dem Beta-Knoten zurückgesetzt werden und der Knoten die Anweisung bekommt, jede Sekunde die aktuellen Zählerstände zum PC zu senden.
-
void getPosition(WorldPosition &p);Die Funktion getPosition liefert die aktuelle Odometrie-Position (pos) zurück, wobei pos.z vom Bogenmaß in Grad umgerechnet wird.
-
int updatePosition();Diese Funktion berechnet aus den Odometrie-Streckenzählern die aktuelle Position des Rollstuhls und bildet damit den Kern dieser Klasse. Der Rückgabewert der Funktion ist bei erfolgreicher Berechnung gleich 0 und für den Fall, daß sich die Räder entgegengesetzt zueinander bewegt haben gleich -1. Der Algorithmus, auf dem die Berechnung beruht, läßt sich wie folgt darstellen:
Zunächst werden die neuen Odometrie-Streckenzähler für das linke und rechte Rad ausgelesen, und es wird die Differenz zu den alten Werten (Streckenzähler beim vorherigen Aufruf von updatePosition) gebildet. In den Variablen odoleft und odoright wird das Ergebnis gesichert. Anschließend muß eine Fallunterscheidung zwischen Kurvenfahrten (odoleft ungleich odoright) und Geradenfahrten (odoleft entspricht odoright) vorgenommen werden, da hier unterschiedliche Algorithmen angewendet werden müssen.
- Kurvenfahrt
-
Voraussetzung für den Kurvenfahrt-Algorithmus ist, daß sich der Rollstuhl nicht auf der Stelle dreht, d.h. odoleft und odoright müssen entweder beide positiv oder negativ sein. Ist dies nicht der Fall wird eine Fehlermeldung ausgegeben. Anderenfalls beruht das weitere Vorgehen darauf, den Punkt P zu bestimmen, um den sich der Rollstuhl gedreht hat und den Winkel alpha, der die Orientierungsänderung darstellt (Vgl. Abb. poslokal). (Abbildung:
Lokale Positionsbestimmung ) Da wir die Differenz der Löcher zwischen dem linken und rechten Rad bezüglich der Rollstuhlbreite kennen, können wir den Abstand (r1) zwischen dem äußeren Rad und P bestimmen: Dreisatz (Bsp. Rechtskurve) Somit ist eine Fallunterscheidung erforderlich, die zwischen Links- und Rechtskurven differenziert, um jeweils das "richtige" äußere Rad in die Rechnung einzubeziehen. Der Abstand (r) zwischen P und unserem Rollstuhlbezugspunkt (Mitte der Vorderachse) ergibt sich jetzt aus r1-centerdist. Um den Winkel alpha zu erhalten, normieren wir den gefahrenen, äußeren Kreisbogen (odoleft bzw. odoright * holediff) auf den Einheitskreis, indem wir durch r1 dividieren. Demzufolge beinhaltet schließlich die Variable alpha die Orientierungsänderung angegeben im Bogenmaß. Nachdem r und alpha bestimmt worden sind, ist eine Unterscheidung zwischen Links- und Rechtskurven nicht mehr erforderlich.Mit Hilfe von alpha und r können nun die folgenden Größen bestimmt werden, die sich jeweils auf die lokale Bewegung des Rollstuhls seit der letzten Berechnung beziehen:
v = Bewegung in y-Richtung s = Bewegung in x-Richtung l = zurückgelegte Strecke tau = Winkel des lokalen x-/y-Versatzes Anschließend erfolgt die Aktualisierung der globalen Position (pos.x, pos.y, pos.z) um die eben berechnete lokale Positionsänderung. Dazu ist es erforderlich die Lage (Winkel gamma) der zurückgelegten Strecke (l) im globalen Koordinatensystem zu bestimmen. (Abbildung:
Vorwärtsfahrt ) Der Winkel gamma kann errechnet werden, indem man zu der noch nicht aktualisierten globalen Orientierung tau addiert. Die Abbildung glbfwd zeigt die Lage des lokalen Koordinatensystems im Verhältnis zum globalen Koordinatensystem bei einer Vorwärtsbewegung des Rollstuhls mit Linkskurve. Aufgrund der absoluten Orientierung des Rollstuhls nach rechts ist pos.z(0) positiv, während der Winkel tau durch die gefahrene Linkskurve negativ ist. Die Addition beider Werte ergibt folglich gamma. (Abbildung:Rückwärtsfahrt ) Daß diese Berechnung auch bei Rückwärtsfahrten korrekt ist, soll in Abbildung glbbwd beispielhaft veranschaulicht werden. Hier ist die Orientierung (pos.z(0)) zu Beginn negativ und der Wert von tau, der jetzt bezüglich der Rollstuhl-y-Achse gemessen wird, positiv (rechts Kurve).Nachdem gamma bestimmt ist, kann die globale x-Position um l * sin(gamma) und die y-Position um l * cos(gamma) aktualisiert werden. Auch die globale Orientierung (pos.z) wird nun um die Orientierungsänderung alpha ergänzt, wobei darauf geachtet wird, daß pos.z im Interval zwischen -Pi und +Pi bleibt. Schließlich wird noch die posChanged-Variable gesetzt.
- Geradenfahrt
-
Bei der Berechnung der neuen Position nach einer Geradenfahrt muß lediglich unter Berücksichtigung der Orientierung (pos.z) der Anteil der gefahrenen Strecke (odoleft * holediff)) in x- und y-Richtung zur globalen Position (pos.x und pos.y) hinzugefügt werden. Ferner ist posChanged auch hier wieder auf true zu setzen.
-
boolean positionChanged();Gibt den Zustand der Variablen posChanged zurück, die angibt ob sich der Rollstuhl seit dem letzten Aufruf dieser Funktion bewegt hat.
-
double distOfPositions(WorldPosition pos1, WorldPosition pos2);Zur Berechnung der Distanz zwischen zwei Positionen nach dem Satz des Pythagoras kann diese Prozedur benutzt werden.
-
angle angleDiff(angle angle1, angle angle2);Berechnet die kürzeste Differenz zweier Winkel, die in Grad anzugeben sind.
-
angle getPolarAngle();Die Funktion getPolarAngle() rechnet die aktuelle x-/y-Position in den Winkel (angegeben in Grad) im Polarkoordinatensystem um.
-
void calibTrans();Diese Prozedur soll das korrekte Setzen der holediff-Variable ermöglichen. Die holediff-Variable enthält den Abstand (in mm) auf dem Reifen zwischen zwei Löchern des Rollstuhlrads und wird z.B. von der jeweiligen Belastung des Rollstuhls und dem Reifen-Luftdruck beeinflußt. Eine Neukalibrierung ist notwendig, wenn die berechnete x-/y-Position weit von der tatsächlichen Position abweicht. Eine Änderung dieser Variable hat keinen Einfluß auf die Güte der Orientierung des Rollstuhls.
Beim Aufruf dieser Prozedur ist sicherzustellen, daß vor dem Rollstuhl keine Hindernisse sind, da der Rollstuhl sich sofort geradeaus in Bewegung setzt. Überdies ist es erforderlich, sich die Startposition des Rollstuhls zu merken. Sobald eine Taste gedrückt wird, hält der Rollstuhl an. Der Benutzer wird anschließend aufgefordert die exakte Strecke einzugeben, die der Rollstuhl zurückgelegt hat. Aus dieser Angabe wird dann der neue holediff-Wert bestimmt, auf dem dann die folgenden Odometrie-Berechnungen beruhen werden.
-
void calibRot();Mit dieser Funktion kann die centerdist-Variable neu kalibriert werden. Der centerdist-Wert gibt den Abstand zwischen Rad und der Rollstuhl-Mitte an. Die Änderung dieses in der Realität konstanten Wertes macht Sinn, da so Einfluß auf die Berechnung der Rollstuhl-Orientierung ausgeübt werden kann, ohne daß eine Auswirkung auf die Korrektheit der x-/y-Position erfolgt.
Voraussetzung für den Kalibrierungsvorgang ist, daß bevor diese Prozedur aufgerufen wird, ein Lenkwinkel von ungleich der Null-Stellung anliegt und vor sowie seitlich von dem Rollstuhl genügend Freiraum zur Verfügung steht. Beim Aufruf von calibRot() fährt der Rollstuhl ebenfalls sofort los. Aus Sicherheitsgründen kann der Rollstuhl durch das Drücken einer beliebigen Taste gestoppt werden. Ansonsten hält der Rollstuhl selbständig an sobald aus der Odometrie-Berechnung folgt, daß sich die Orientierung um mind. 90 Grad geändert hat. Danach hat der Benutzer die tatsächliche Orientierung anzugeben, die als Grundlage für die Ermittlung des neuen centerdist-Werts dient.
double holediff;
Diese Variable beinhaltet den Abstand (gemessen auf dem Reifen und angegeben in mm) zwischen zwei Löchern des Rollstuhlrads.
double centerdist;
Der Wert von centerdist gibt den Abstand zwischen Rad und Rollstuhl-Bezugspunkt (Mitte der Vorderachse) an.