von Jannis Wacker und Martin Kappen

ProjectBOB ist ein Projekt, in dem es um die Steuerung eines ferngesteuerten Quadrocopters (nachfolgend „BOB“) geht. Statt eines Menschen, der die Flugbahn des Copters sieht, ein Ziel wählt und den Copter manuell über die Joysticks der Fernbedienung lenkt, wird hier eine Kamera genutzt, um aus den Bildern die Copterposition und seine Geschwindigkeit zu bestimmen und über einen Arduino die Befehle an die Fernbedienung weiterzugeben. BOB soll autonom und ohne menschliche Kontrolle fliegen koennen.

Zum Semesterbeginn haben wir uns folgende konkrete Ziele gesteckt:

• Kameraortung von BOB
• Automatisierter stabiler Flug
• Start - und Landung auf Knopfdruck
• Informationsausgabe in Koordinaten

Die wahren Ziele haben sich aber erst während der Entwicklung herausgestellt. So sind die Kernanforderungen geblieben, dass BOB stabil fliegen kann und farblich automatisch erkannt wird. Das Herz des Projekts ist jedoch die Optimierung des Prozesses, während Banalitäten wie Datenausgabe oder Knopfbelegungen zwar zuerst ein Meilenstein waren, im Endeffekt jedoch keine große Errungenschaft.

Schematische Darstellung des Projektaufbaus

Wie im Schema angedeutet, wird BOB von einer Kamera im Raum erfasst. Die Bilddaten werden dem Laptop zugeschickt, der sie auswertet, darstellt und daraus errechnete Befehle an den Arduino Microcontroller weitergibt. Dieser ist direkt mit der Fernbedienung verbunden und gibt somit die Befehle zurück an BOB. Die Eingabe erfolgt schlussendlich gar nicht mehr an der Fernbedienung, lediglich ein Mausklick auf dem Interface reicht aus, um BOB stabil an den Zielort fliegen zu lassen.

Ein großer Teil unserer Arbeit zu Beginn des Projektes war simple Recherche und der Versuch, die Fortschritte der „pFalcon“ Gruppe zu verstehen.

Der Quadrocopter Rayline R807V, den wir aus unersichtlichen Gründen BOB getauft haben, stellt mehr Funktionen bereit als anfangs vermutet. Sowohl das selbstständige Ausgleichen des Copters bei schräger Raumlage als auch der Speedmodus sind äußerst hilfreich für das Funktionieren unserer Steuerung. Der Akku hält etwa 10 Minuten und braucht ca. 30 Minuten zum Aufladen. Der Plastikrahmen schützt sowohl die Rotoren als auch Personen vor Schäden, verbiegt sich jedoch sehr leicht. Standardmäßig sind vier grüne LED-Reihen an den Armen des Copters angebracht. Zusätzlich eine weiße LED vorne und eine blaue LED im Zentrum auf der Platine.

Der gegebene Freaduino, eine Variante des Arduino Uno, war unkompliziert zu nutzen. Der wichtigste Unterschied zum Arduino Nano sind der Schalter, durch den man zwischen 3,3 V und 5 V Spannung wählen kann, und die Tatsache, dass der Uno ein wenig leistungsfähiger ist. Da die Joysticks im Bereich 0 V bis 3,3 V arbeiten, ist der Arduino Uno, bzw. der Freaduino für diese Aufgabe besser geeignet.

Die Funktionsweise der Fernbedienung zu erfassen war eine erste große Schwierigkeit. Die Auswirkung der Joystickstellung und die Pulsweitenmodulation zu testen hat viel Zeit in Anspruch genommen. Außerdem kam es durch die starke Beanspruchung der Fernbedienung häufiger zu Wackelkontakten. Die Idee, den Ansatz unserer Vorgänger nicht zu nutzen und stattdessen eine eigene Platine mit den nötigen Schaltkreisen mit dem Sender der Fernbedienung zu bauen, mussten wir leider fallen lassen. Der Ansatz würde zwar sämtliche Nachteile der jetzigen Fernbedienung vermeiden, jedoch ist es uns nicht möglich die Übertragung zwischen dem Micro Controller der Fernbedienung und dem Sender vollständig nachzuvollziehen und nachzubilden.

Die Pulsweitenmodulation, kurz PWM, ist die beste Möglichkeit mit einem Arduino Spannungswerte zwischen LOW (0 V) und HIGH (3,3 V) zu erhalten. Dies imitiert die manuelle Steuerung mit den Joysticks, bei der je nach Stellung ein Regelwiderstand so gestellt wird, dass intern die Spannung auf einen Wert zwischen 0V und 3,3V gesetzt wird (s.u.: Abb. LT-Spice) Bei der PWM wird mit einer sehr hohen Taktung ständig zwischen LOW und HIGH gewechselt. Der prozentuale Anteil der Zeit, in dem HIGH geschaltet ist, entspricht dem prozentualen Anteil von 3,3 V, der im Durchschnitt ausgegeben wird. Folgendes Diagramm verdeutlicht das Prinzip:

Quelle: https://www.pantechsolutions.net/images/stories/k2-sourcecode/sinusoidal-pwm-output-pulses.jpg

Die Kondensatoren und Widerstände auf dem Shield, das am Arduino angebracht ist, bilden drei verschiedene Tiefpassfilter um die drei verschiedenen Pins jeweils von der Pulsweitenmodulation auf einen konstanten Durchschnittswert zu regeln. Welcher Pin an welchem Kabel endet, haben wir in folgender Zeichnung dargestellt:

Schaltung des Arduinos und des Tiefpassfilters

Wie man sieht, nutzen wir den Pin 3 um die rechts-links-Achse anzusteuern, den Pin 9 für die vorwärts-rückwärts-Achse und mit dem Pin 10 lässt sich die vertikale-Achse beeinflussen.

Nachdem die Pulsweitenmodulation vestanden war, konnten wir die einzelnen Komponenten (der Arduino, der Tiefpassfilter auf dem Shield und die Fernbedienung) miteinander verbinden, wodurch die Kommunikation vom Arduino zum Quadrocopter ermöglicht wurde.

Im Prinzip wird so eine Spannung an den Sender der Fernbedienung geleitet, damit BOB ein Signal empfangen kann, ohne dass die Joysticks bewegt werden muessen. Dazu sind die Pins 3, 9 und 10 (x-, y- und z-Bewegungsachsen) des Arduinos mit der Unterseite der Joysticks verbunden. Die Joysticks selber fungieren als regelbare Widerstände, die je nach Auslenkung der Knüppel variieren. Dadurch gibt es nun zwei Stromquellen, den Arduino und die Batterien der Fernbedienung, die beide auswirkungen auf den Signalstrom haben, der die Flughöhe bestimmt. Das gesammte Schaltbild, der beiden Stromquellen, dem Tiefpassfilter und dem Joystick sind auf dem folgenden Bild zu sehen:

LTspice-Model der Schaltung

Der linke Teil der Schaltung ist der Arduino mit Widerstand und Kondensator, der rechte Part ist Teil der Fernbedienung. Rvcc und Rgnd sind Variable Widerstände, die von der Position des Joysticks abhängen. Diese Abhängigkeit zeigt sich in der folgenden Messung, bei der auf der Y-Achse die Spannung in Volt, bei konstanten 3,3 V Teilspannung seitens des Arduinos zu sehen ist. Die X-Achse zeigt den Ausschlagswinkel des Joysticks in Grad:

Winkel-Spannungs-Diagramm

Wie man sieht, ist der Graph nicht linear, weswegen die richtige Einstellung des Hebels wichtig ist. Wir haben als Standardjoystickstellung für unsere Versuche 80 Grad, also 2/3 der vollen Auslenkung, gewählt. Das heißt zu Beginn eines Durchlaufs muss der linke Joystick (Gashebel) manuell auf diesen Wert gestellt werden. Dies ist nötig, da der entsprechende Hebel für die vertikale Auslenkung keine Feder besitzt und somit nicht immer auf einen Ausgangswert zurückspringt.

Somit erhalten wir bei einer maximalen Ausgabe von 3,3 V des Arduinos eine Spannung von etwa 3,1 V, die an dem Microchip in der Fernbedienung anliegt.

Oben: Lötstellen zwischen Arduino und Joysticks. Unten: Fernbedienung und Arduino auf fixiert auf einem Brett

Um den Quadrocopter mit der Kamera gut erkennen zu können, ist eine der besten Lösungen ihn farblich zu makieren. Da dabei jedoch die Verwechslung mit anderen farblichen Objekten möglich ist, haben wir Tischtennisbälle an den Plastikrahmen von BOB angebracht, die von innen mit bunten LEDs beleuchtet werden. Dadurch sind die Bälle in Farbe und Helligkeit spezifischer als die meisten Hindernisse, oder Kleidungsstücke. Die LEDs sind mit dem Akku des Quadrocopters verbunden, wobei wir im Falle des roten Balles die Stromstärke mithilfe einiger Widerstände auf 20 mA runterregeln mussten. Der grüne Ball wird durch eine der vier LED Reihen angestrahlt, die standartmäßig am Copter angebracht sind.

Der Quadrocopter mit Beleuchtung

Die Software war ein umfassender Teil des Projekts und hat es stetig begleitet. Anfangs ging es vorerst darum unabhängig vom Quadrocopter, Farben auf dem Bildschirm zu erkennen und damit zu arbeiten. Zur Farberkennung ist es wichtig, das Kameraprofil richtig einzustellen, da nur so die Farben richtig erkannt werden. Dieses erarbeitete Prinzip konnte dann auf BOB angewednet werden, um ihn zunächst korrekt darzustellen. Folglich war der nächste Schritt, diese Bilddaten zu verarbeiten. Aus einem roten und einem grünen Punkt lassen sich Bewegungsrichtung, Höhe, Geschwindigkeit und Ausrichtung errechnen, was es ermöglicht BOB in seiner elementaren Bewegung zu steuern. Lediglich die Neigung fällt hier raus, ist jedoch für dieses Projekt eher vernachlässigbar.

Diese errechneten Werte lassen sich in Steuerungssignale (PWMs) umwandeln und an den Arduino via serieller Schnittstelle senden. Der Arduino widerum benötigt ein spezielles Programm, damit die Kommunikation mit dem Computer einwandfrei funtioniert (BOBFirmata2.ino).

Konkret haben wir als Editor Qt mit openFrameworks verwendet, das bereits viele nützliche Funktionen zur Verfuegung stellt. Es lassen sich einfache Objekte erzeugen, die es unkompliziert ermöglichen, beispielsweise auf dem Bildschirm zu zeichnen, den Arduino zu verbinden oder grafische Berechnungen anzustellen (Dokumentation: www.openframeworks.cc/documentation). Vor allem macht es oF sehr einfach bestimmte Events, wie z.B. einen Tastendruck auf der Tastatur zu triggern und einzubinden.

Einige Funktionen im Detail:

• Initialisierung des Programms: void setup()
• Standardprogrammablauf in Dauerschleife: void update()
• Grafische und rechnerische Kamerabildauswertung: void updateScreen()
• Bewegungsfunktion: void moveToTarget()

Hier wird alles nötige initialisiert: die Größe des Ausgabebildschirms und Kleinigkeiten, wie einige Variablen. Die PS3 Kamera wird verbunden (vidGrabber.setDevideID(x), wobei die Zahl für x vorher in der Konsole entnommen wird, falls andere Geraete zur Auswahl stehen) und der Arduino wird verbunden (arduino.connect(„'PORT'“, 'Baudrate') .

In SetupArduino(), wird die Firmata Firmware geöffnet und die für uns wichtigen Pins (3, 9, 10) als PWM Output definiert. Zuletzt wird auf dem 10er Pin noch einmal eine manuelle Initialisierung der Fernbedienung simuliert, die BOB mit der Fernbedienung verbindet.

Die update() Funktion entspricht dem loop() eines Arduino Sketches, diese Funktion wird immer hintereinander weg aufgerufen und nur durch Events, wie ofApp::keyPressed() unterbrochen.

Zuallererst prüft das Programm, ob sich das Kamerabild überhaupt verändert hat und ob folglich eventuell gar keine Änderung in der Steuerung veranlasst werden muss. Im Normalfall geschieht dies jedoch. Dann wird geprüft ob BOB im Bild ist und nicht Pause gedrückt wurde. Danach wird die Bewegungsfunktion 'moveToTarget()' aufgerufen (s.u.). Für den Fall, dass BOB sich nicht (mehr) im Bildausschnitt befindet, leitet das Programm eine Notlandung ein.

In updateScreen() passiert alles, was mit Farberkennung und Kamerabildauswertung zu tun hat. Das Bild wird durch ein Pixelarray dargestellt, in dem die Pixel zeilenweise durchnummeriert sind. Es ist also enstprechend einem Koordinatensystem mit dem Urpsrung in der oberen linken Ecke. Dies macht sich der Algorithmus zu Nutze um alle Pixel der Reihe nach durchzugehen. Dabei findet eine Prüfung nach der Farbe jedes Pixels statt (jeder Pixel besteht in sich aus nochmal drei Punkten, den Farbwerten rot, grün und blau (rgb): Jede Farbe ist mit einem Wert zwischen 0 und 255 angegeben). Die gefunden Farbwerte des Pixels werden dann zwischengespeichert und im nächsten Schritt mit den vom Benutzer ausgewählten Farben verglichen (eine gewisse Toleranz ist vorhanden). Ist der Check positiv, wird der Pixel in der gewünschten Farbe dargestellt, andernfalls ist er schwarz.

Per Default sind ein Rot- und Grünton als Vergleichsfarbe eingespeichert, die unter den meisten Lichtverhältnissen BOBs LEDs erkennen. Sollte dies nicht funktionieren, kann die Vergleichsfarbe per rechten Mausklick auf einen Bildpunkt neu angewählt werden.

Da die Tischtennisbälle im Kamerabild mit vielen Pixeln dargestellt werden, muss zur Positionsbestimmung zunächst ein Mittelwert aller Koordinaten der entsprechenden Punkte ermittelt werden. Dies geschieht mit Hilfe des Arithmetischen Mittels aller gefunden X- und Y-Werte der jeweiligen Farben. Unter normalen Umständen fällt Bildrauschen nicht ins Gewicht.

Die Höhe wird berechnet, indem wir den Mittelpunktabstand der Beiden Farbbälle berechnen. Diesen Abstand geben wir zunächst nur in Pixeln an, theoretisch lässt er sich jedoch noch in Meter umrechnen.

Der horizontale Drehwinkel des Quadrocopters ergibt sich aus dem Arctan des Y-Abstandes der beiden Bälle geteilt durch den entsprechenden X-Abstand. Mit Hilfe dieses Winkels können wir nun ein zweites Koordinatensystem erstellen, durch das die Rotation des Quadrocopters in die Bewegungsfunktion miteinberechnet werden kann.

Schließlich findet hier noch ein Test statt, ob BOB sich überhaupt im Bild befindet, also genügend rote und grüne Bildpunkte vorhanden sind, was notwendig ist um ggf. die Notlandung einzuleiten.

Hier findet der interessante Teil statt, der BOBs Bewegung steuert. Letztendlich haben wir nur Kontrolle über drei Pulsweitenmodulationen, welche die drei Bewegungsachsen ansteuern. Doch die Feinjustierung der gesendeten Werte beruht auf mathematischer Auswertung des Kamerabildes.

So soll BOB beispielsweise mehr Gas geben, je weiter er vom Ziel entfernt ist, muss jedoch andererseits vor erreichen des Ziels schon wieder abbremsen um nicht darüberhinwegzuschiessen. Wir kontrollieren also nicht seine Geschwindigkeit, sondern seine Beschleunigung. Diese errechnet sich aus dem Abstand zum Ziel unter Berücksichtigung der momentanen Geschwindigkeit, multipliziert mit fürs Tuning relevanten Konstanten, die den errechneten Wert optimal für den Wertebereich 0-255 anpassen sollten. In der Praxis stellt gerade das Feintuning eine grosse Herausforderung dar. Die beiden Parameter müssen so angepasst werden, dass BOB schnell reagiert, Impulse ausgleichen und somit stabil in der Luft stehen kann, aber auch, dass er sich zügig zum Ziel bewegt und letztlich auch nicht darum herum pendelt.

Für die Berechnungen mussten wir auch eine Mögliche Drehung von BOB mit einbeziehen. Da wir keine direkte Steuerung darüber haben und wegen billiger Verarbeitung zufällige Drehungen ziemlich häufig vorkommen, mussten wir BOBs Bewegungen unabhängig seiner Ausrichtung zum Kamerabild errechnen. In der ausgelagerten Funktion getCalc() werden alle nötigen werte berechnet. Die Position der farbigen Bälle, die Distanz dazwischen, die Position des Quadrocopters und der Abstand zum Ziel, welcher der Position in einem neuen Koordinatensystem entspricht.

Beispiel für das Urpsrungskoordinatensystem.

Das alte Koordinatensystem ist das der Kamera. In diesem wird z.B. 'CopterPos' angegeben. Die Position repräsentiert einen Pixel. Das neue Koordinatensystem nimmt BOBs Position als Urpsrung und ist nach seiner Ausrichtung gedreht. Dieser Drehwinkel wird ebenfalls in getCalc() berechnet.

Beispiel für das neue Koordinatensystem. Die Achsen drehen sich mit der Copterausrichtung.

Jetzt lässt sich der Abstand zum Ziel sehr einfach auslesen. Dazu wird die Zielposition ebenfalls in das neue Koordinatensystem übertragen. Die x- und y-Koordinaten aus BOBs Sicht, sind gleichzeitig der Abstand.

Zuletzt werden die PWM-Werte noch mit dem Trimwert (Default 127) addiert, da die Joystickstellung in der Mitte - also kein Bewegungsimpuls - genau der Mittelwert zwischen 0 und 255 ist.

In vertikaler Richtung misst das Programm den Abstand zwischen den beiden Farbbällen und nutzt dies als Höhe. Eine unreife Version der automatischen Hoehenerhaltung befindet sich bereits im Programm, es ist aber noch sehr Fehleranfällig (vor allem, je hoeher BOB fliegt). Dies resultiert aus einem schwankenden Maßstab, BOB scheint umso schneller, je näher er der Kamera ist, da bereits kleinste Bewegungen viele Pixel 'zurücklegen'. Auch das Trimmen fällt hier schwer, da der Quadrocopter einer ständigen Abwärtsbeschleunigung ausgesetzt ist (Schwerkraft) und vor allem schwankende Akkuleistungen (abhängig vom Ladezustand) diesen Ausgleich erschweren.

Das Prinzip das wir anwenden hat einige Schwachpunkte, die wir teilweise lösen konnten. Die Software ist lauffähig, bietet aber natürlich noch unendlich Raum zur Optimierung, um BOB präziser und stabiler fliegen zu lassen. Hier listen wir zunächst auf welche möglichen Probleme man im Blick behalten muss.

Nachfolgend ist eine Visualisierung der Ausgaben im Log zu sehen. Mit sechs ausgegebenen Werten kann man einen Flugverlauf ziemlich einfach nachvollziehen.

Wie man den PWM-werten entnehmen kann, ist der Flug anfangs ziemlich ruhig. BOB bewegt sich zwar etwas neben dem Ziel (sichtbar am Abstand), hält sich aber stabil. Ab 4200ms ist eine plötzliche Schwankung sichtbar, zunächst und am stärksten schlagen die Geschwindigkeiten aus. Dies könnte man als Impuls auf den Copter interpretieren, z.B. durch äußerere Einwirkung. Realistischer ist aber wohl, dass BOB sich im Laufe des Fluges immer weiter nach oben bewegt hat (eine entsprechende Höhenausgabe fehlt leider) und dann von der Kamera fälschlicherweise als schnell wahrgenommen wurde. Kurz darauf erreichen auch die PWMs ihre Maximalwerte (0-255(!)) und versuchen die vermeintliche Bewegung auszugleichen. Das funktioniert noch nicht sher zuverlässig. Laut der Daten fängt sich BOB ein wenig, ist aber auch am Ende noch nicht wieder stabilisiert.

Der Graph gibt wertvolle Informationen für das Feintuning der Bewegungsparameter. Sie müssen so angeglichen werden, dass starke Ausschläge schnell und sicher wieder abgefangen werden, aber dennoch der Abstand zum Ziel nicht zu gross wird.

Seit die Bildbearbeitung Funktioniert und die Befehlsübergabe über die Serielle Schnittstelle steht, konnten wir einige Fortschritte in der Optimierung verzeichnen. Die nächsten Schritte, die wir auf Grund des Semesterendes nicht mehr durchführen können sind hier näher beschrieben.

Eines von BOBs schwierigsten Problemen, ist die ungenaue automatische Konfigurierung, die der Copter durchführt, nachdem die Fernbedienung und das Programm gestartet wurden. Tut man dies manuell mit der Fernbedienung, muss man den Joystick einmal komplett nach vorne schieben und anschließend wieder auf den niedrigsten Wert ziehen. Im Programm durchgehen wir den gleichen Ablauf, nur eben digital mit dem Arduino. Die Ungenauigkeiten die bei dem Konfigurationsvorgang ablaufen führen häufig dazu, dass der Copter sich ein Stück weit neben dem Zielpunkt einpendelt. Mit Hilfe eines PID-Reglers, würde sich diese Abweichung jedoch selbst ausgleichen. Dafür müssen wir die Beschleunigungen in X und Y Richtung nicht nur in Abhängigkeit des Abständen und der Geschwindigkeit, sondern auch in Abhängigkeit des Integrals der Abstände berechnen.

Die entsprechende Änderung im Programmcode würden folgendes beinhalten müssen:

• Deklaration in der Header Datei

long targetDistSum[2];

int p3;

• Berechnung in der getCalc Funktion

targetDistSum[0] += targetDist[[0];

targetDistSum[1] += targetDist[[1];

• Einbeziehen in die PWM-Ausgabe

pwmX = targetDistSum[0] / p3 + p2 * (p1 * targetDist[0] - (oldPosX -center[0])/(ofGetElapsedTimef() - oldTime));

pwmY = targetDistSum[1] / p3 + p2 * (p1 * targetDist[1] - (oldPosY -center[1])/(ofGetElapsedTimef() - oldTime));

Nach dem gleichen Prinzip könnte man auch die Höhenfunktion optimieren. So erhält man nicht nur eine exaktere Höhe, sondern schwächt sogar den Einfluss des Akkuzustands aus.

Um die Höhe des Copters in jeder Lage exakt bestimmen zu können, müssten wir noch einen dritten optischen Punkt anbringen, durch den wir alle sechs Freiheitsgrade berechnen könnten. Dies würde deutlich mehr Stabilität in den Flug bringen und starke Fehleinschätzungen des Programms vermeiden. Sobald man zuverlässige Höhenwerte hat, kann man durch den Strahlensatz einige Fehler ausgleichen, die in der Skalierung entstehen, da wir nur eine Kamera haben.

Um Fehlerquellen beim Start zu entgehen, empfehlen wir, BOB genau nach diesen Schritten zu starten. Die allermeisten Probleme lassen sich dadurch vermeiden.

1. Die Kamera und den Arduino mit dem Laptop verbinden
2. Das Firmata Programm auf den Arduino laden (BOBFirmata2.ino)
3. Kameraweißabgleich einstellen:
   1. (Linux mit guvcview): GUVCView installieren. Kommandozeile 'guvcview -o -d /dev/video1' ausführen. Im nun geöffneten Fenster Settings → Load Profile → ProjectBOB.gpfl.
   2. Andere Plattformen: Äquivalente Programme finden
4. Den linken Joystick auf 2/3 nach vorne schieben (auf gleicher Linie mit dem roten Strich links am Fuß des Joysticks)
5. Fernbedienung anschalten und BOB auf den Boden stellen
6. Auf der Fernbedienung den oberen der vier Knöpfe auf der linken Seite drücken für „High speed“
7. Programm starten
8. Falls Farben nicht automatisch erkannt werden: Farben auf Monitor mit rechter Maustaste anwählen (ggf. BOB näher an die Kamera halten)
9. Gewünschte Zielposition auf Monitor mit linker Maustaste auswählen

projectbob.zip