der
Translationsvektor ist. Ein Anfängerpraktikum im Studiengang Anwendungsorientierte Informatik (Bachelor)
Universität Heidelberg, Wintersemester 2006/2007
Hauptaufgabenstellung war es, einen Weg zur automatischen (stapelbasierten) Programmierung des NXT zu finden. Der Hintergrund ist die Absicht, NXT-Roboter zur Ausführung von Problemlösungen zu bringen, die aus verschiedenen Gründen nicht direkt auf dem NXT berechnet werden können. Die Lösung des Problems sollte also extern auf einem "großen" Computer errechnet und anschließend automatisch in ein entsprechendes NXT-Programm generiert werden.
NBC (Next Byte Codes) ist eine Assembler-ähnliche (d.h. low-level) Programmiersprache zur Entwicklung von Software für die originale Virtual Machine des NXT. Bei NXC (Not eXactly C) handelt es sich um eine Erweiterung der Sprache um C-ähnliche Konstrukte.
NextTool, ein Kommandozeilenprogramm für Windows, bietet eine primitive Benutzungsschnittstelle für die Funktionen des NXT- Schnittstellentreibers. Der Umfang an Funktionen zur Wartung des NXT-Steins entspricht weitgehend dem der offiziellen Entwicklungsumgebung NEXT-G, wenngleich die Bedienung mangels GUI natürlich umständlicher ist. Der Vorteil ist, dass NextTool einfach per Kommandozeilenbefehl aus anderen Programmen heraus aufgerufen werden kann. Dies eröffnet die Möglichkeit, Operationen am NXT-Stein zu automatisieren.
Das Traveling Salesman Problem (Problem des Handlungsreisenden) ist in Mathematik und Informatik ein bekanntes Beispiel für ein so genanntes NP-schweres Problem. Einfach ausgedrückt bedeutet dies, dass das Problem einen besonders hohen Lösungsaufwand erfordert, insbesondere steigt der Aufwand zur Lösung für eine Eingabegröße n mit wachsendem n überproportional an, was dazu führt, dass NP-schwere Probleme häufig schon für "eigentlich" geringe Eingabegrößen nicht mehr praktisch lösbar sind.
Es geht darum, dass für eine gegebene Menge von Städten mit bekannten Koordinaten der kürzeste Weg gefunden werden soll, auf dem jede Stadt einmal besucht wird.
Aufgrund der NP-Schwere des Problems handelt es sich um einen idealen Beispielfall für unsere Aufgabe, da man zu seiner Lösung mangels effizienter Algorithmen auf pure Rechenleistung angewiesen ist. Der NXT-Prozessor hätte keine Chance, das Traveling-Salesman-Problem selbst für eine geringe zweistellige Anzahl von Städten in akzeptabler Zeit zu lösen, die "Auslagerung" der Lösungsberechnung auf einen leistungsfähigeren Computer ist zwingend.
Die wichtigste Information, welche zur Lösung des Traveling Saleman Problems benötigt wird, ist natürlich die Kenntnis über die Lage der zu besuchenden Städte. Wir modellieren die Städte als rote, etwa handtellergroße Kreisflächen, die über ein ca. 1x1 Meter großes Spielfeld verteilt werden. Die Koordinaten der Punkte möchten wir automatisch mit Hilfe einer Kamera und eines Bildauswertungsprogramms ermitteln. Um die realen Koordinaten der Punkte anhand der Koordinaten ihrer Pixel auf dem aufgenommenen Bild zu bestimmen, haben wir uns des Lochkameramodells bedient.
Das Lochkameramodell basiert auf einer perspektivischen Projektion der 3D-Koordinaten auf eine 2D-Bildebene.
Zunächst müssen die 3D-Koordinaten aus dem Koordinatensystem W(X Y Z) mit den Basisvektoren (x,y,z) des Kamerakoordinatensystems dargestellt werden. Der Basiswechsel besteht aus einer Rotation und einer Translation (affine Verschiebung des Koordinatensystems). Um sowohl Rotation als Auch Translation als lineare Transformation zu schreiben, bedient man sich der homogenen Koordinaten.
Homogene Koordinaten sind normale Koordinaten, die um eine Dimension erweitert wurden und bei denen folgende Äquivalenzrelation existiert:
Eine Rotation in homogenen Koordinaten :
wobei rij eine normale Rotationsmatrix ist.
Eine Translation in homogenen Koordinaten:
wobei der
Translationsvektor ist.
Die Koordinatentransformation bestehend aus Rotation und Translation:
Bei der Perspektivischen Projektion werden die realen Koordinaten in C (x, y, z) auf die Bildebene wie folgt projiziert (Strahlensatz):
Oder in Homogenen Koordinaten:
Die Gesamte Transformation ergibt sich also zu:
Die Matrix F lässt sich invertieren. Jedoch ergibt die resultierende Matrix keine Abbildung aus R² in den R³, sondern es ist eine Abbildung von R² in eine Sichtlinie („line of sight“). Diese Geraden müssen dann mit einer Ebene (der Spielfläche) geschnitten werden, um die 3D-Koordinaten zu ermitteln.
Die inverse Transformation in homogenen Koordinaten ergibt sich zu:
mit
. Wobei Die Ebene in W-Koordinaten gegeben ist durch h0 + nh1 + mh2. Ziel der Kamerakalibrierung ist es, (FH)-1 zu erlernen.
Bei der Umsetzung waren die Ergebnisse mit dem Ersten Algorithmus nicht zufriedenstellend, weshalb wir auf eine zweite Methode zurückgegriffen haben. Es werden dennoch beide Methoden vorgestellt und die Probleme mit der ersten beschrieben.
Mit der Kamera wird ein Raster aus roten Punkten mit vorgegebenem Abstand aufgenommen. Die Zentren dieser Punkte werden bestimmt. Mit einem Algorithmus wird ein Zuordnungsvorschrift zwischen „Real World“- und Pixelkoordinaten bestimmt. Diese wird dann bei der Durchführung des Salesmanroboterproblems benutzt, um die Positionen der Städte, des Roboters und seine Ausrichtung zu bestimmen.
Seien yi Koordinaten im W-Frame und dessen Abbildungen ai auf der Bildebene bekannt. Gesucht sind die Parameter
} mit
, d.h. der quadratische Fehler zwischen berechneten und bekannten Koordinaten soll minimiert werden. Diese Minimierungsfunktion wurde in Matlab mit Hilfe der eingebauten Funktion lsqnonlin implementiert. Der Haken and dieser iterativen Methode ist die Startwertbestimmung der Parameter. Beim "lsqnonlin"-Löser handelt es sich um einen Gradientenabstiegsverfahren. Bei schlechten Startwerten kann der Algorithmus in einem lokalen Minimum konvergieren.
Wir haben probiert die Startwerte per Hand abzuschätzen, was von Anfang an suboptimal war. Der Fehler in der Kalibrierung lag im besten Fall bei 5cm. Meistens aber knapp unter . Beim genaueren lesen von [1], und nachdem wir [2] gefunden hatten fanden wir heraus dass diese Methode nur dann sehr gute Ergebnisse Liefert, falls die Initialisierung der Parameter gut ist. Die erzielten Genauigkeiten lagen dann bei ~cm. Die Initialwerte, erhält man z.B. über die zweite Methode. Mit dieser Methode war der Fehler in der Kalibrierung im Millimeterbereich, was bei der Fahrungenauigkeit des Roboters völlig ausreichend ist.
Die Idee ist, direkt die Transformationsmatrix F zu fitten. Im Anschluss wird (FH)-1 bestimmt. F ist eine 3 x 4 Matrix, ausgeschrieben:
Bei der Kalibrierung sind die (X Y Z 1)T und die
dazugehörigen (ui vi 1)T bekannt. 
muss eliminiert werden . Dies
ist gerade gegeben durch
Die einzelnen Komponenten ausgeschrieben und umgeformt ergeben:
Ähnlich gilt:
Um beide Gleichungen in eine zu schreiben, definiert man:
f' = (f11 f12 f13 f14 f21 f22 f24 f31 f32 f33 f34)Tund
und es gilt damit:
L * f' = 0Diese Gleichung gilt für exakt bestimmte Pixelkoordinaten. Da jedoch die Positionbestimmung immer fehlerbehaftet ist brauchen wir eine Lösung im „least squares“ Sinn. Dazu muss die zu der obigen Gleichung zugehörigen Normalengleichung bestimmt werden.
LTL * f' = 0Um die Triviale Lösung f’ = 0 zu vermeiden, muss eine Normalisierung angewendet werden. [2] schlägt f34 = 1 vor. (Was wohl aus Eigenschaften der homogenen Koordinaten hervorgeht. Dies ist aber nicht unbedingt ersichtlich).
Anmerkung: Dadurch, dass für unser Problem kein wirkliches
3D-Problem vorliegt, sondern alle zu Kalibrierende und zu Bestimmende Punkte auf
einer Ebene liegen und man ein Koordinatensystem wählen kann, in der Z immer 0
ist. Bestimmen wir eigentlich 
, was jedoch egal ist, da wir später, diese Matrix mit eben dieser
Ebene (H) schneiden (s.o.).
Die Kamera liefert RGB Bilder. Uns interessiert vor allem der Rotkanal, in der sich die Städte und der Roboter besonders hervorheben. Dieser Kanal wird betrachtet. Es handelt sich dabei um eine N x M Matrix mit Integer wertigen Einträgen.
Dieser Kanal wird mittels Medianfilter und Gaussfilter zunächst entrauscht.
Danach wird
.
Damit werden
alle Schwarz – Grau anteile aus dem Rotkanal entfernt.
Anmerkung: Dieser Schritt wurde eingeführt um den unterschiedlichen Belichtungsverhältnissen entgegenzuwirken. Der schwarze Untergrund wirkt unter Sonnenlicht fast weiß.
Als letztes wird aus dem Rot Kanal eine binäre Matrix der gleichen Größe erzeugt indem die Werte normierten Werte auf oder abgerundet werden.
Es wird als nächstes entlang der Dimensionen der Matrix aufsummiert. Vergleichen kann man das mit einem Schattenwurf. Man erhält dadurch zwei Vektoren der Größe n und m. Die Spalten der Matrix , in denen sich keine Städte (und damit rote Pixel) befinden werden sich als 0 im aufsummierten Vektor bemerkbar machen. Als nächstes werden die Bereiche bestimmt, in denen sich die Projizierten Bereiche überlappen (links in dunkelgrau).
Das Verfahren zeigt seine Grenzen, wenn Die Städte zu nah beieinander sind:
Auf einem Din-A0-Bogen wird ein Raster wie in der obigen Abbildung gedruckt. Der Abstand zwischen den roten Punkten ist bekannt, und damit die yi. Ins Zentrum eines roten Punktes wird der Ursprung gelegt. Die restlichen Punkte ergeben sich aus geometrisch hen Überlegungen. Der Din-A0-Bogen wird mittels einer Videokamera mit Analog-zu-Digital-Wandler aufgenommen und die ai bestimmt. Danach werden diese Koordinaten in der DLT verwendet. Die Kalibrierung hatte einen Fehler im Millimeterbereich.
Nachdem in der Kalibrierung die Transformation zwischen Pixel und 3D Koordinaten bestimmt wurde wurde die eigentliche Problemstellung gelöst. Auf dem Spielfeld werden Rote Punkt als Position der Städte markiert. Und der Roboter auf das Feld gesetzt.
Um nochmal zu wiederholen, was gemacht wurde:
Am Roboter ist ein roter Punkt angebracht, so dass seine Position als Stadt
miterkannt wird. Zusätzlich besitzt der Roboter eine grüne Zunge. Der center of
mass des Grünanteils wird (nach ähnlicher Vorverarbeitung wie mit dem Rotkanal)
bestimmt und unter den Koordinaten der Roten Punkte die Koordinate
herausgesucht, die sich am nächsten befindet. Da der grüne und der rote Punkt
eine feste räumliche Beziehung haben kann daraus die Ausrichtung des Roboters
bestimmt werden.
Die Stadtpositionen werden mit obigem Algorithmus
bestimmt. Die Pixelkoordinaten werden Transformiert und die 3D Koordinaten sowie
die Ausrichtung des Roboters in eine CSV Datei geschrieben.
Wir haben nun also die Koordinaten der zu besuchenden Städte ermittelt. Jetzt gilt es, das Traveling-Salesman-Problem für diese Städte zu lösen. Aus technischer Sicht bedeutet dies, die im vorigen Schritt ermittelten Koordinaten in die richtige (d.h. der Anforderung des TSP gerecht werdende) Reihenfolge zu bringen.
Zur Lösung des TSP verwenden wir einen primitiven, Stack-basierten Algorithmus, welcher einfach sämtliche möglichen Wege testet, um den kürzesten zu ermitteln. Der Algorithmus wurde in einem C++-Programm implementiert, welches die unsortierten Koordinaten aus einer CSV-Datei liest, in der Reihenfolge des kürzesten Weges anordnet und im gleichen Format wieder in eine Ausgabedatei schreibt. Er arbeitet auf folgende Weise:
Durchlaufe den Stack und überprüfe, ob ein Weg vollständig ist (d.h. ob alle Städte besucht werden).
Wenn ja, füge den letzten Teilweg (von der letzten Stadt zurück zur Ausgangsstadt) hinzu und überprüfe, ob dieser Weg kürzer ist als derjenige, der bisher als kürzester gemerkt wurde.
Wenn ja, merke den neuen Weg als bisher kürzesten und verwerfe den bisherigen.
Wir haben also nun Koordinaten als Ausgabe des TSP-solvers vorliegen, daraus müssen wir nun ein Programm erzeugen, dass der von uns ausgesuchte Compiler (siehe Anhang: NXC/NBC) übersetzen kann in Motorenbefehle. Die sortierten Koordinaten liegen als CSV-Datei vor (Text-Datei mit durch Komma getrennte Werte), und werden nacheinander von einem Programm eingelesen und Zeile für Zeile in mehrere NXC-Befehle übersetzt.
Das Programm zur Übersetzung hierfür wurde in C++ verfasst (der komplette Quellcode befindet sich im Anhang). Der Programmablauf lässt sich mit den folgenden Punkten zusammenfassen:
1. Einlesen des Ausrichtungsvektors 2. Berechnung der Ausrichtung des Roboters zu Anfang 3. Einlesen des ersten Wegpunkts 4. Einlesen des folgenden Wegpunkts 5. Berechnung der Drehung/der Entfernung zwischen den Koordinaten 6. Schreibe passende Fahrbefehle in Ausgabepuffer 7. Wiederhole 4., 5. und 6. bis zum Ende der Datei
Die genauen Funktionen und Aufrufe sind im Quellcode nachzulesen (siehe Anhang).
NXC (Not eXactly C) ist ein Aufsatz für den freien NBC-Compiler um mit einer C ähnlichen Sprache komfortabler zu programmieren. Die Syntax ist fast genau wie bei C, daher auch der Name. Um das Kalibrieren des Roboters zu vereinfachen, sind die eigentlichen Fahrbefehle in Makros verpackt. Das bedeutet eine einfache Ersetzung des Codes durch vorher definierte Funktionen. Somit sieht der generierte Code immer gleich aus, obwohl sich die genauen Fahrbefehle bzw. Kalibrierung ändern können.
Hier am Beispiel des DRIVE()-Makros:
DRIVE(x) (x ist die
Distanz in Zentimeter) wird ersetzt durch:
RotateMotorEx(OUT_BC, 60,
21*(x), 0, true); ResetAllTachoCounts(OUT_BC);
Wait(10)
(aus NXTmacros.h, zu finden im Quellcodepaket im
Anhang)
In Worten ausgeschrieben: Drehe Motoren B und C synchron mit 60% Kraft um 21*x Grad vorwärts. Zurücksetzen des Rotationszählers und warten von 10 ms.
Ein ähnliches Makro existiert auch für den Befehl zur Drehung (TURN(s, x), s ist 1 oder -1 für links oder rechts, x die Drehung in Grad). Nun also haben wir korrekten NXC-Code in der Ausgabedatei, der nur noch vom Compiler übersetzt werden muss.
Der NBC-Compiler wird vom Codegenerator aufgerufen und übernimmt das kompilieren und übertragen des Programms auf den NXT (per Bluetooth oder USB).
Man kann den NBC-Compiler natürlich auch direkt per Kommandozeile aufrufen (wie im Bild oben). Nach dem Übertragen ist das Programm auf dem NXT gespeichert und kann per Knopfdruck am NXT oder per Software gestartet werden.