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 15 cm 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 5x5 m grosse Spielfeld legen. Das Spielfeld ist in 1x1 m grosse Felder unterteilt. Punkte gibt es für das Platzieren eines Würfels in einem solchen Feld.

ein Bild der Arena des SICK Robot Days 2016. Die Arena ist etwa 13x7 m groß und hat die oben genannten bereiche. Es liegen gelbe und rote Würfel in der Arena.

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 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:

Wir verwenden die Messungen des RFID Lesers um den Offset zwischen map und odom zu berechnen.

eine Handskizze der beschriebenen Koordinatensysteme in einer draufsicht auf die Arena

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.

eine Handskizze die den Roboter, das Ziel sowie den Messbereich das LiDAR zeigt. Außerdem sind zwei hindernisse Eingezeichnet, die von dem LiDAR erkannt werden.

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.

hier sind drei Graphen aufgezeichnet, die jeweils von minus pi halbe bis plus pi halbe gehen. Der rechte Graph ist dabei die Multiplikation der ersten beiden. Im ersten Graphen ist für jeden Winkel der gemessene Abstand aufgetragen. Im mittleren zweiten Graphen ist eine breite Gauskurve abgebildet, die in richtung des Ziels den höchsten Punkt hat.
hier sind auch drei Graphen aufgezeichnet, die jeweils wieder von minus pi halbe bis plus pi halbe auf der x-Achse gehen. Der erste Graph ist das Ergebnis der vorherigen Multiplikation aus dem Bild davor. Der mittlere Graph ist eine spitzere Gauskurve. Diese beiden Graphen werden miteinander gefaltet und bilden das Ergebnis des letzten rechten Graphen.

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:

zu sehen ist ein Node Graph, der verschiedene States mit Pfeilen (aktionen) verbindet. Die States sind: Select and drive to cube, Approach and take cube, Deploy cube, Drive to storage area, Drop cube and Error (Drive near center). Wenn bei irgendeinem State etwas schief geht, oder ein Timeout passiert, kommt man in den Error State und der Roboter fährt in die Mitte der Arena.

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.

zu sehen ist ein Bild von Beteigeuze mit ein paar Kamaros drum herum. Ein Mitglied baut am Greifer rum.

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.

Der Zeitraffer von unserem Lauf beginnt bei 1:42.

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

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.

Teamfoto mit Beteigeuze und dem Sonderpreis in der Hand