SICK robot day 2016
Die Aufgabenstellung
Der SICK robot day wird alle zwei Jahre vom Sensorhersteller SICK ausgerichtet. Er findet in einer Halle statt, die Aufgabe ist aber jedes mal eine andere.
Dieses Jahr ging es um den Transport und das Platzieren von Würfeln mit 15cm Kantenlänge. Es treten jeweils zwei Roboter gegeneinander an. Jeder Roboter hat einen Lagerbereich aus dem er Würfel entnehmen kann. Diese muss er dann auf das zentrale 5x5m grosse Spielfeld legen. Das Spielfeld ist in 1x1m grosse Felder unterteilt. Punkte gibt es für das Platzieren eines Würfels in einem solchen Feld.
Wie jedes Jahr bekam man von SICK einen Sensor gestellt, der einem beim Lösen der Aufgabe helfen soll. Dieses mal einen RFID Leser zur Lokalisierung. Dafür waren unter dem Spielfeld RFID Tags im Abstand von 1m an bekannten Positionen platziert.
Unsere Lösung
Lokalisierung
Zur Lokalisierung standen uns die Encoder der Motoren, eine IMU (Kompass, Accelerometer und Gyroskop) und der RFID Sensor zur Verfügung. Da man aus den Motorencodern relativ schlecht auf die Rotation des Roboters schliessen kann (die Lenkung verhält sich nicht ganz linear und nicht ganz deterministisch), hatten wir bereits ein Modul entwickelt das die Encoder und die IMU kombiniert indem es die aus der IMU kommende Rotation in die aus den Encodern berechneten Bewegungen einfliessen lässt.
Da die so berechnete Position über die Zeit natürlich global gesehen ungenau wird, wollten wir diese mit den Messungen den Messungen aus dem RFID Sensor kombinieren die einem eine global korrekte Position geben. Zuerst wollten wir dafür ein Kalman-Filter verwenden mit Hilfe vom pykalman. Das haben wir aber nicht in angemessener Zeit zum Laufen bekommen und stattdessen einen etwas anschaulicheren Ansatz gewählt:
Nach dem Standart ROS TF Layout haben wir drei wichtige Koordinatensysteme:
- base_link: Liegt im Mittelpunkt des Roboters
- odom: Ein aus der aus IMU und Encodern berecheten Odometrie berechnetes Koordinatensystem das fest in der Welt liegt, aber über die Zeit abdriftet.
- map: Ein fest in der Welt liegendes Koordinatensystem das nicht abdriftet.
Wir verwenden die Messungen des RFID Lesers um den Offset zwischen map und odom zu berechnen.
Aus einer Messung des RFID Lesers bekommen wir immer zwei korrespondierende Punkte. Einen im odom Koordinatensystem (Die Position des Roboters bei der Messung) und einen im map Koordinatensystem (Die bekannte Position des RFID Tags auf dem Spielfeld).
Mittels z.B. SVD Zerlegung können wir dann die Transformation zwischen map und odom Koordinatensystem berechnen.
Damit die Transformation immer aktuell bleibt und den Drift der Odometrie kompensieren kann verwenden wir immer nur die letzten 20 gesehenen RFID Tags.
Bewegung(splanung)
Die Bewegung mit unserem Roboter ist relativ kompliziert, da er wie ein Auto gelenkt wird und sich nicht auf der Stelle drehen kann.
Zuerst wollten wir zur Bewegungsplanung das teb_local_planner Modul von ROS verwenden, da dies solche Lenkungen unterstützt. Damit sollten wir also im Prinzip in der Lage sein einen bestimmten Punkt auf dem Spielfeld anzufahren.
Das hat auch funktioniert, aber nicht so zuverlässig wie nötig. In leicht komplizierteren Situationen konnte sich der Planer oft nicht für einen eindeutigen Weg entscheiden und oszillierte zwischen mehreren Lösungen hin und her. Auch waren Rangiervorgänge meistens sehr langwierig (was auch an unserer ungenauen Lenkung lag). Ein weiteres großes Problem war, dass unser Greifer den Würfel nicht anhebt sondern nur festhält und so den vorderen Lidar verdeckt. Es wäre aufwändig gewesen dem Planer beizubringen das korrekt zu beachten.
Also haben wir uns dazu entschieden einen eigenen simpleren Ansatz zu verwenden, der die Bewegung nicht im Voraus plant, sondern nur die aktuelle Situation betrachtet. Den haben wir bei der Robotour bereits zweimal verwendet.
Für die Navigation verwenden wir die Abstandsmessungen vom Lidar oder den Ultraschallsensoren (wenn der Lidar vom Würfel verdeckt ist) und die Richtung zum Ziel (aus der Lokalisierung berechnet). Diese wandeln wir jeweils in eine Verteilung um die angibt, wie sinnvoll es ist in die jeweilige Richtung zu fahren.
Wir multiplizieren die beiden Verteilungen
Und falten sie mit einer Gauss-Funktion um die Ungenauigkeit der Lenkung und die Breite des Roboters (da die Methode eigentlich davon ausgeht, dass der Roboter punktförmig ist) zu berücksichtigen. Der höchste Wert in dieser Verteilung gibt dann die Richtung an in die wir fahren sollten, in diesem Fall stark rechts.
Hier ist auch nochmal die Erklärung dieses Prinzips von der Robotour.
State Machine
Der Gesamtablauf wird von einer State-Machine gesteuert, die in etwa so aussieht:
Hier haben wir auch einige Probleme die auftreten können behandelt, sodass der Roboter wenn etwas fehlschlägt meistens eine gute Chance in einem neuen Versuch hat.
Der Wettbewerb
Probleme vor Ort
Wie eigentlich bei jedem Wettbewerb sind auch hier vor Ort einige Probleme aufgetreten mit denen wir nicht gerechnet haben.
Zuerst erkannte der RFID Sensor viele Tags fehlerhaft. Nachdem wir einige Zeit mit verschiedenen RFID Tags den Output unseres Programms an verschiedenen Stellen angeschaut hatten, konnten wir den Fehler finden: Ein nicht ganz korrekter sscanf Aufruf zum Lesen der Tag ID aus der Kommunikation mit dem RFID Sensor der mit den Tags die wir zum Testen verwendet hatten zufällig funktionierte.
Nun waren wir soweit auf dem Spielfeld testen zu können wo wir auf ein weiteres unerwartetes Problem stießen: Die Reibung des Teppichbodens in der Arena war so hoch, dass der Roboter die Würfel nicht von der Stelle bewegen konnte. Ein an die richtige Stelle auf dem Greifer geklebter Pappwulst sorgte dann aber dafür dass der Würfel beim Greifen leicht nach oben rutschte und so nicht mehr am Boden fest hing.
Erster Lauf
Kurz vor dem Start des ersten Laufs hatte auf einmal der vordere Lidar keinen Strom mehr. Nach einem Neustart des Roboters und dem öffnen einer Klappe (Wackelkontakt) funktionierte er dann wieder und wir konnten starten. Darauf schaffte es der Roboter dann nicht all zu schnell aber ziemlich problemlos vier Würfel zu platzieren. Etwa vier Minuten vor Ende blieb er aber einfach stehen, da sich die Stromversorgung des Lidars wieder gelöst hatte.
Im offiziellen Video von SICK ist ein schöner Timelapse unseres ersten Laufs.
Zweiter Lauf
Zwischen den beiden Läufen hatten wir das Problem mit der Stromversorgung des Lidars gelöst und der Roboter konnte direkt starten. Nachdem er der ersten Würfel platziert hatte verklemmte er sich aber zwischen diesem Würfel und dem gegnerischen Roboter, der sich wohl aufgehängt hatte. Nach ein paar Minuten in denen nichts passierte, entfernte das gegnerische Team netterweise ihren Roboter und unser Roboter konnte, etwa wie im ersten Lauf, noch drei weitere Würfel platzieren.
Ergebnis
Für das Siegertreppchen hat es leider gerade so nicht gereicht (Kamaro: 4P, EDURO (3.): 4,5P, Cogito Mart(1.): 7P IIRC). Aber wir haben einen Sonderpreis für konstant gute Leistung (4P in beiden Läufen) bekommen, worüber wir uns natürlich auch sehr gefreut haben.
Keine Kommentare