Kameranavigation

Kameranavi

Einführung

Um den Zeppelin erfolgreich autonom steuern zu können, ist ein robustes und genaues Navigationssystem erforderlich. Die Ultraschall&Infrarot Lösung (Gemeinsames IPS) ist zwar schon vorhanden, aber noch nicht optimiert. Es könnten zB Probleme mit Reflexionen auftreten.
Bei der ersten Besprechung der Spezifikationen und möglichen Lösungen unserer Gondel kamen wir auf die Idee, ein kamerabasiertes System zu benutzen. Nach kurzen Überlegungen zur einfachsten Implementation hatten wir zwei Tage später eine Einachsige Punktverfolgung mit Matlab, eine Woche später Punktlokalisierung für zwei Kameras und Punktverfolgung für beliebig viele Kameras.
Die steile Erfolgskurve ließ ein vernünftiges System erwarten.

{autotoc addNumbering=1|addNumberingToc=true}

Idee

Die Grundsätzliche Idee ist, mithilfe von Triangulation einen Punkt im Raum zu bestimmen. Als unser System schon die ersten Tests hinter sich hatte, erfuhren wir, dass ein Projektpraktikums-Team im Vorjahr eine ähnliche Lösung verfolgt hatte und gescheitert war (Bewegungs-Erkennung, Parallaxe).

Unser Ansatz, obwohl ebenfalls trianguliert wird, beruht nicht auf der Erkennung von Bewegung, sondern auf der Filterung, Erkennung, und Verfolgung von Infrarotlicht.
Da das Menschliche Auge Infrarotlicht nicht sehen kann, besitzen Kameras in der Regel ein Infrarot-Sperrfilter, das dafür sorgen soll, dass das Kamerabild dem menschlichen Sehen entspricht (Dass man trotzdem das Licht von zB IR-Fernbedienungen sehen kann, liegt an der geringen Qualität der eingesetzten Filter). Dieses Filter kann entfernt werden. Dadurch ist das Bild der Kamera "verfälscht" und grüne Blätter erscheinen weiss (Wood-Effekt).
Wenn man jetzt ein IR-Bandpassfilter hinzufügt, ist das Bild komplett schwarz, außer es erfasst eine IR-Quelle, wie zB eine Fernbedienung oder Sonnenlicht.

Source: http://wwwintra.rcs.ei.tum.de/~dietrich/rtes/TUM2013_Tutorials/10_BUS_with_handnotes.pdfUnter anderem weil das Kamerabild verzerrt ist, scheint es eine gute Idee zu sein, den Infrarotpunkt genau in der Mitte des Bilds zu haben, da dort keine Verzerrung sein sollte. Dazu muss der Punkt durch mechanische Drehung der Kamera verfolgt werden. Ein weiterer positiver Effekt davon ist, dass der Öffnungswinkel der Kamera nicht mehr kritisch ist, auch wenn die Positionserkennung in geringer Entfernung funktionieren soll.

Die tatsächliche Positionsberechnung funktioniert dann über Triangulation.

Verworfene Konzepte

Theoretisch muss man die Kameras nicht hacken, wenn man sichtbares Licht trackt. Um die Erkennung zu vereinfachen, muss der Hintergrund abgezogen werden. Man kann den Zeppelin mit der LED mit dem Navigationssystem synchronisieren und die LED nur für genau jeden zweiten Frame einschalten. Die Frames werden dann voneinander abgezogen und es bleibt die Helligkeit der LED.
Allerdings gestaltet sich die Synchronisation schon ohne Funk schwierig. Eigentlich müsste man das VSync Signal abgreifen, aber wenn man schon Hardwaremodifikationen machen muss, kann man auch die IR Methode benutzen.
Ein weiteres Problem ist, dass wenn sich der Hintergrund bewegt (zB durch Kameradrehung, Zeppelinbewegung oder vorbeilaufende Personen), sieht man die Kanten der Bewegung. Das macht die Erkennung eines Punktes deutlich schwieriger.

Funktioniert unbewegt ganz gutBei Bewegung sieht man aber leider viele Artefakte

Etwas spät fanden wir heraus, dass WiiMotes (~8€) kleine Kameras beinhalten, die dafür ausgelegt sind, eine kleine Anzahl von IR-LEDs zu orten. Damit wäre eventuell einiger Entwicklungsaufwand weggefallen. Einige Dokumentation gibt es im entsprechenden Artikel bei WiiBrew.

Implementation

Hardware

Als Kameras werden PS3 Eyes benutzt. Diese bieten bei einem geringen Anschaffungspreis (~16€) eine gegenüber normalen billig-Webcams extrem hohe Framerate (bis zu 125fps) bei akzeptabler Auflösung (bis 640x480 ohne Interpolation). Diese Cam wird besonders gerne bei Eigenbau-Multitouch-Tischen eingesetzt, daher gibt es Dokumentation in größerem Umfang.
Das Entfernen des IR-Sperrfilters gestaltet sich aufgrund der speziellen Optik relativ Schwierig. Gegen das Fokus-Problem, das nach entfernen des IR-Filters einer PS3 Eye auftritt, wurde ein in Form des IR-Filters gebrachtes Stückchen CD-Case-Plastik eingesetzt. Abrubbeln mit Benzin und Alkohol wie zum Teil beschrieben funktioniert nicht.
Um das nicht-IR Spektrum auszublenden ist ein geeignetes Filter nötig. Es gibt professionelle Bandpassfilter und Eigenbaulösungen aus Fotonegativen, Disketten und VHS-Tape. Fotonegative lassen zu viel sichtbares Licht durch. Mehrere Lagen machen das Bild verschwommen. VHS-Tape ist sehr gut geeignet, schluckt aber wahrscheinlich auch einen großen Teil IR-Licht, weswegen die Reichweite stark begrenzt wird. Professionelle Bandpassfilter gibt es zB im Ebay-Store von omegabob2 oder von Thorlabs.

Thorlabs war so freundlich uns mit 4 940nm Bandpassfiltern zu unterstützen.

Am Zeppelin befindet sich die Infrarotlichtquelle in Form von einigen IR-LEDs, die uns von Conrad zur Verfügung gestellt wurden. Die LEDs haben laut Datenblatt ihre Peak Emission Wavelength bei 940nm. Forward Voltage ist 1.3V und Continuous Forward Current ist 350mA (also 455mW). Der Öffnungswinkel ist 120° (also sehr weit!), wobei der Halbwinkel laut Diagramm bei ±50° liegt.

Die X- und Z-Achsen Servos sind gut zu erkennen, ebenso der IR-BandpassfilterUm die Kameras zum erkannten Infrarotpunkt zu drehen, werden (Modellbau-)Servos eingesetzt. Die Kameras können so in horizontaler und vertikaler Richtung um bis zu ±60° geschwenkt werden. Ein Mini Maestro 24-Channel USB Servo Controller erzeugt die Servosignale. Der Servo Controller wurde freundlicherweise gestellt von Watterott. Details zur Ansteuerung des Servocontrollers via USB befinden sich weiter unten.

Die Funkübertragung der Daten zum Zeppelin wird über einen XBee mit USB-Adapter gelöst. Diese wurden uns ebenfalls von Watterott gestellt.

Nach einigen Versuchen mit einem älteren PC (Erste Athlon-64 Generation) wurde klar, dass der USB-Datendurchsatz der Kameras ein Bottleneck darstellte. USB-Hubs verschlechterten den Durchsatz enorm, ein PCI-USB-Controller verbesserte ihn, ein weiterer verschlechterte ihn. Auf dem leistungsstarken privaten PC eines Teammitglieds lief das System aber einwandfrei und äußerst flüssig.

Eine Kamerahalterung wurde gebaut.

Software

Das Kamerasystem ist in C++ geschrieben. Es werden die  Libraries OpenCV 2.4.5 und Boost 1.5.3 verwendet. Der Code wurde bis jetzt nur auf Linux getestet, sollte aber prinzipiell auch unter Windows funktionieren. Unter Windows wird zusätzlich ein Kameratreiber benötigt und die Namen der Seriellen Schnittstellen müssen angepasst werden.

Ablauf

Im Hintergrund das Navianzeigefenster.Zuerst werden die Anzahl der verfügbaren Kameras und die Anwesenheit von XBee und ServoController registriert. Danach werden in der Hauptschleife Kamerabilder gelesen und verarbeitet. Dabei wird ein Fenster mit allen Bildern und Informationen angezeigt. Dieses kann mit der ESC-Taste geschlossen und das Programm damit beendet werden.

Die Kamerabilder werden zeitgleich gelesen (durch Verwendung von retrieve() und grab() statt read()) und in den jeweiligen NaviCam-Objekten gespeichert. Dass das Kamerabild ohne Infrarotlichtquellen (beinahe) schwarz ist, erleichtert die Erkennung sehr stark. Im Moment wird ein Infrarotpunkt erkannt, wenn der hellste Pixel eines Bilds mehr als die Standardabweichung der Pixelwerte vom Mittelwert der Pixelwerte abweicht.

Ein I-Regler versucht den Ort dieses hellsten Pixels (sofern er als IR-Punkt erkannt wurde) für jede Kamera in die Bildmitte zu bekommen. dazu werden nach der Analyse jedes Frames neu berechnete Servopositionen an den Servo Controller gesendet. Es muss leider ein Regler mit ausschließlichem I-Anteil verwendet werden, da wir die momentane Position eines Servos nicht wissen, sondern nur seine Zielposition. Dadurch kann zB schlecht die Geschwindigkeit des Servos abgeleitet werden für einen D-Anteil, der die Einschwingzeit sicher deutlich verbessert hätte. Wie unter Potentiale beschrieben, gäbe es unter einigem Aufwand dafür aber eine Lösung. Auf dem schnelleren PC läuft der I-Regler aber sehr gut.

Wenn mindestens zwei Kameras einen Punkt erkennen, wird die Position trianguliert und gefunkt.

Wenn eine Kamera für eine kurze Zeit lang (60 Frames) keinen Fix bekommen hat (also keinen IR-Punkt gefunden hat), geht sie in einen Such-Modus. Dazu wird in jedem Frame, in dem kein Fix erfolgt, ein Counter inkrementiert. Ab dem Wert 60 wird dieser für eine Spiralfunktion in der Form t*sin(t) / t*cos(t) mit auf-und abklingender Amplitude verwendet. Die Kameras finden dadurch den Zeppelin nach kurzer Zeit, wenn sie (zB wegen ausgeschaltetem IR-Modul am Zeppelin) abgedriftet waren.

Die Navisoftware beherrscht neben der Übermittlung der Positionsdaten auch die wichtigsten Kommandos der Funkkommunikation.

Es wird aus dem Positionsverlauf noch die Bewegungsrichtung des Zeppelins ermittelt. Bei Kenntnis der wahren Drehungsrichtung mit einem Kompass / Gyro könnte so eine Drift-erkennung implementiert werden. Da wir gegen den Drift aber mit unserer Gondelhardware eigentlich nichts unternehmen können, wird dieser Wert zwar übermittelt, aber nicht benutzt.

Triangulation

Die Berechnung der Position des Zeppelins geschieht über Triangulation.

Die Kameras stehen an bekannten Punkten im Raum. Ihre Blickrichtung ist ein dreidimensionaler Vektor. Aufgrund von Ungenauigkeiten werden sich die "Blick-Strahlen" der Kameras in 3D nicht schneiden, sie sind windschief. Das zu lösende mathematische Problem ist daher, den Punkt im Raum zu finden, der von allen Strahlen den geringsten Abstand hat.

Geometrisch ist dieses Problem bei zwei Kameras mit einer Hilfsebene lösbar. Das ganze in einer Programmiersprache zu implementieren ist aber zu aufwendig.

Im Moment wird zur einfacheren Berechnung folgender Algorithmus eingesetzt: Für jedes mögliche Paar aus der Menge der Kameras, die einen Lichtpunkt erkannt haben, werden ihre Blick-Strahlen auf die XY-Ebene projiziert. Dort besitzen sie (fast) sicher einen Schnittpunkt. Dieser kann durch gleichsetzen der Geradengleichungen berechnet werden (und zwar auch in C++). Die Höhe des Punktes kann dann durch Trigonometrie mit einem der Strahlen der Kameras einfach errechnet werden. Aus den berechneten Positionen wird ein Mittelwert gebildet. Der mittlere Abstand der berechneten Positionen vom gemittelten Punkt wird ebenfalls berechnet und ergibt ein Maß für die Genauigkeit des berechneten Punktes.

Ein alternativer Ansatz wäre, Punktwolken zu verwenden. Dafür werden die Strahlen durch viele Punkte auf ihnen dargestellt, ein Algorithmus wie FLANN sucht dann den Punkt mit dem geringsten Abstand zu allen anderen Punkten auf allen Strahlen.

Hardware/Software Schnittstelle

Um die große Zahl Servos (2 pro Kamera) anzusteuern wird ein USB Servo Controller von Watterott benutzt. Der Kontrollport des Controllers registriert sich am PC als /dev/ttyACM0. In jedem Schleifendurchlauf werden neue Servo-Positionen an den Controller geschickt. Dazu wird ein Byte-Paket aufgebaut, wie im Manual unter "5.e. Serial Servo Commands" (S.39) beschrieben. Während der Programminitialisierung werden zusätzlich noch etwaige Error-Flags des Controllers gecleared. Am Programmende wird ein Go-Home-Paket gesendet, damit die Servos in ihre Mittelstellung zurückfahren.

Ein USB-Adapter wird benutzt werden um ein XBee Pro Funkmodul anzusteuern, das die Daten zum Zeppelin überträgt. Das XBee registriert sich glücklicherweise nicht mit dem selben Namen wie der Servocontroller, sondern als "/dev/ttyUSB0". Alles was man dorthin sendet, wird per Funk übertragen.

Software-Installation

Diese Anleitung ist für Ubuntu Linux. Die Installation dauert insgesamt etwa 1h. Adminrechte sind erforderlich, um die libraries und includes in /usr/local unterzubringen und um die Toolchain zu installieren falls nicht vorhanden.
Das Kamerasystem benutzt die Boost Library für Serial Communication und die OpenCV Library um Bilder aufzunehmen und zu analysieren.

Requirements besorgen

Die Source ist in der Dropbox und im Anhang der Dokumentation. Boost gibt es in den Ubuntu-Paketquellen, OpenCV muss scheinbar manuell kompiliert werden.
OpenCV:http://sourceforge.net/projects/opencvlibrary/files/opencv-unix/2.4.5/opencv-2.4.5.tar.gz/download
Archiv herunterladen und zB ins Home-Verzeichnis (~) entpacken.

# Pakete besorgen
sudo apt-get install build-essential g++ cmake pkg-config libgtk2.0-dev libboost-all-dev
OpenCV installieren
# In das Verzeichnis wechseln in das OpenCV extrahiert wurde
cd ~/opencv-2.4.5/

# Build-Ordner erstellen
mkdir release

# In diesen wechseln
cd release/

# Cmake in .. aufrufen. Der Navi-PC ist so alt, dass er keine SSE3-Instruktionen versteht.
cmake -D ENABLE_SSE3=NO ..

# Builden (dauert eine Weile, auf dem Navi-PC 45min)
make

# In /usr/local installieren
sudo make install
Kompilieren der Source

Navi-Sourcecode (.cpp Datei) aus der Dropbox oder dem Anhang der Dokumentation kompilieren mit:

g++ Kameranavi.cpp -o Navi.out -std=c++0x -lopencv_core -lopencv_highgui -lopencv_imgproc -lboost_system -lpthread

Die Datei Navi.out kann jetzt ausgeführt werden.

Potentiale

Im Moment sind auf einem starken PC etwa 30 Lokalisierungen pro Sekunde möglich. Leider bietet OpenCV keinen Support für die Einstellung der Framerate der PS3 Kameras. Sonst wären bis zu 125Hz Updaterate möglich. Da 30Hz aber für einen Zeppelin ausreichen, ist fraglich ob sich der Aufwand lohnt, eine andere Library dazwischenzuschalten oder Alternativen zu OpenCV zu finden und sich darin einzuarbeiten.

Die Winkel die zur Positionsbestimmung verwendet werden, sind im Moment nur die an den Servos eingestellten Winkel. Der Infrarotpunkt befindet sich aber gar nicht unbedingt in der Mitte des Kamerabilds, da der Regler und die Servos eine kurze Zeit brauchen um hinterherzuregeln. Man könnte die Pixel-Position des Punkts auch in die zur Positionsbestimmung herangezogenen Winkel miteinberechnen, um genauere Ergebnisse zu bekommen.

Der verwendete Servo-Controller hat Sonderfunktionen, mit denen man Spannungen an den "Servoausgängen" messen kann. Damit könnte man die Spannung am Ausgang des Potentiometers im Servo messen um ein direktes Positionsfeedback zu bekommen. Die Regelung der Kameraposition könnte dann mit P und D Anteilen ausgestattet werden und wäre dann deutlich schneller und genauer.

Fazit

Abschließend kann man sagen, dass das Kameranavigation eine für unsere Ansprüche sehr gute Performance liefert. Während die Auflösung im Sub-Millimeterbereich liegt, haben wir in Tests bei 5m Entfernung eine Wiederholgenauigkeit von 8mm festgestellt. Die Linearität des Koordinatensystems wurde zwar angenommen, spielt aber keine große Rolle, da wir unsere Wegpunkte mit dem selben System aufnehmen. Die Reichweite ist absolut ausreichend, wir haben bei etwa 20m Entfernung immer noch gute Positionsdaten ermittelt, mehr wäre sicherlich noch möglich.

Der Entwicklungsaufwand dieser Lösung war sicherlich höher, als wenn wir das vorhandene IPS-System mitverwendet hätten. Durch die Unabhängigkeit ging die Entwicklung aber sehr schnell und wir hatten im Gegensatz zu den anderen Teams sehr früh ein funktionierendes System.

Weitere Bilder

Früher Versuchsaufbau zur Punktverfolgung mit MatlabSpäte Tests vor dem Wettflug

(Dateien befinden sich unter Übersicht > Downloads)

Anhänge:
Zugriff auf URL (https://www.dropbox.com/s/k0a6bgkwnwsig0b/Kameranavi.cpp)Kameranavi.cpp[Code]%2013-%07-%09 %1:%Jul%+02:00