Datenauslese

Der Grund, warum die Datenauslese einen eigenen Abschnitt bekommt ist folgender:

Wir haben einen 1056 – PhidgetSpatial 3/3/3 als Inertial Measurement Unit (IMU) auf der Drohne installiert. Diese Platine ist mit neun Sensoren bestückt, drei linearen Accelerometern, drei Gyroskopen und drei Magnetfeldsensoren. Das Magnetfeld wollen wir vorerst nicht bestimmen, deshalb werden wir den letzteren keine Aufmerksamkeit schenken. Die Lage im Raum bestimmen wir über die Accelerometer (ACC) und die Gyros. Das erste Ziel, welches wir mit der Drohne erreichen wollen, ist das automatische Schweben. Dieses Ziel kann im Prinzip rein über die Auslese der ACC-Daten erfolgen, jedoch werden die Ergebnisse verfälscht, sobald höhere Drehraten auftreten. Dies ist der Fall für unruhige Luft (also im Prinzip immer, sobald man außerhalb von geschlossenen Räumen ist). Dann müssen die Daten mit Hilfe der Gyros korrigiert werden. Wie das genau passiert, das möchten wir in diesem Abschnitt erklären.

Grundlegendes:

Zunächst benötigen wir für unsere Berechnungen ein einheitliches Koordinatensystem. Bei der Definition desselben folgen wir den Konventionen, welche auch für den Flugzeugbau gelten. Hierbei wird der Rollwinkel als die Rotation um die Längsachse bezeichnet. Positiv roll bedeutet somit eine “Rechtskurve”. Dasselbe gilt für den Pitchwinkel. Dieser gibt die Rotation um die Querachse an, positiv nach oben. Hier eine Illustration:

 

Damit wäre ein Standard geschaffen, besser adaptiert. So können wir die gewonnenen Daten an mehreren Stellen verarbeiten.
Nun zur Datenerfassung.
Die ACC-Daten liefern uns die aktuell gemessene Beschleunigung in drei Raumachsen in Einheiten der Erdbeschleunigung [g]. Das heißt, wenn die x und y Achsen 0g liefern und die z-Achse 1g, dann betragen der Roll und der Pitch-Winkel jeweils 0°, also liegt die Drohne horizontal in der Luft. Man kann also die Lage tatsächlich so bestimmen. Wenn nun hohe Winkelgeschwindigkeiten (ab ca. 3°/s) auftreten, dann werden die Messdaten, wie erwähnt durch Trägheitseffekte verfälscht. Dies kann man sich wie im Auto vorstellen. So lange das Auto steht ist es einfach sich die Lage des eigenen Körpers über den Gleichgewichtssinn festzustellen. Werden jedoch Kurven gefahren oder die Beschleunigung steigt über eine gewisse Schwelle, dann braucht man den Horizont um sich zu orientieren. Dasselbe gilt für die IMU. Die Accelerometer können so die exakte Lage bei starken Beschleunigungen nicht mehr messen. Unsere Drohne kann den Horizont nicht feststellen, jedoch sehr exakt die Drehungen um die drei Raumachsen. Diese Drehraten nutzen wir um unser ACC-Signal zu korrigieren. Die Messdaten werden von den Gyros des Spatial in [°/s] geliefert. Diese müssen dann integriert werden um die Winkel zu erhalten. Die numerische Integration passiert bei uns als Multiplikation der aktuellen Drehrate mit der vergangenen Zeit seit der letzten Messung. Als Referenz wird der letzte ACC-Wert genommen.

Zur detaillierteren Beschreibung der einzelnen Berechnungen.
Die statischen Roll- und Pitch- Werte werden, wie schon erwähnt, aus den ACC-Messwerten bestimmt. Diese werden vom IMU-Modul (Instanz des IModule) aus dem Phidgets Spatial ausgelesen. Dies erfolgt sequentiell und liefert zusätzlich einen Zeitstempel, welche die vergangene Zeit seit der letzten Auslese ist. Danach werde diese drei Messwerte über den Ansatz eines Rechtwinkligen Dreiecks mit Hilfe der Pythagorasschen Formeln verrechnet um den Roll- und den Pitchwinkel zu erhalten. Folgendes Bild veranschaulicht die Zusammenhänge:

Konkret bestimmen bestimmen wir Roll und Pitch über folgende Gleichungen:

Was aus der Zeichnung nicht hervorgeht ist der Nenner des Arkustangens. Solange eine Verkippung um eine Achse vorliegt reicht es, wenn man alpha = atan(y/z) rechnet. Liegt jedoch eine Verkippung um zwei Achsen vor, so projiziert sich ein Anteil des Vektors der Gewichtskraft auf x UND y. Deswegen muss der Nenner korrigiert werden. Diese Berechnung liefert uns nun den Roll- und Pitchwinkel unserer Drohne.

 

Diese Werte müssen jetzt korrigiert werden. Dafür integriert man die Rotationen. Numerisch realisieren wir dies durch die Multiplikation der Rotationswerte mit der gemessenen Zeit. Dies liefert uns den “zurückgelegten Winkel”, also die Winkeländerung zwischen den beiden Messpunkten. Dieser Differenzwert wird auf den vorher gemessenen ACC Winkel aufaddiert. So erhalten wir die Rohdaten der Roll- und Pitchwinkel.
So weit so gut, wir haben also die Daten erfasst und können sie weiter verarbeiten. Aber wie sehen diese Daten aus? Hier ein Plot von reellen Daten, die während eines Testlaufs genommen wurden. Die 27% Motorleistung entsprechen etwa der Abhebeleistung unseres Kopters.

 

alt

Wie man sehen kann sind die Daten extrem mit Rauschen behaftet und in der Tat sind die PID-Regler mit diesen Daten nicht in der Lage die Drohne zu stabilisieren. Sie schaukelt sich eher noch mehr auf. Das Rauschen resultiert aus den Vibrationen, die die Motoren auf den Rahmen ausüben. Diese pflanzen sich auf die IMU fort und verändern die Messwerte der einzelnen ACC-Achsen. Um dennoch eine saubere Flugregelung programmieren zu können, müssen die Rohdaten vor der Verarbeitung vorprozessiert werden. Das heißt, die Daten müssen gefiltert werden.

Datenfilterung bedeutet in unserem Falle, die Daten zu glätten und so grobe Ausschläge zu entfernen. Der untere Plot zeigt das Ziel einer Filterung, eine ideale Messwertkurve. Diese Kurve folgt aus einer offline Fourieranalyse. Hier wurden die Rohdaten in den Frequenzraum transformiert und die hochfrequenten Schwingungen entfernt. Eine anschließende Rücktransformation zeigt dann das folgende Bild.

alt

Das gezeigte Verhalten ist im Prinzip das Resultat eines idealen Tiefpassfilters. Das heißt, der Filter lässt alle tiefen Frequenzen durch und schneidet alle hohen Frequenzen ab. Die online-Fourieranalyse ist auf unserer Plattform jedoch nicht zu implementieren, da die Performance zu schlecht wäre. Wir haben für den obigen Plot 8192 Messwerte offline transformiert, abgeschnitten und wieder Rücktransformiert. Dies dauert auf einem Core i5 mit 2,4 GHz 0,65s. Somit ist dieser Weg auf dem kleinen ARM des SBC nicht möglich.
Das Erstellen eines eines Tiefpassfilters welcher fast das ideale Ergebnis liefert ist also das Ziel unserer Bemühungen. Dieses Verhalten ist durch einen FIR-Filter zu erreichen. Mathematisch folgen wir der Definition:

Hierbei werden als Filterparameter alpha_k die Werte der Impulsantwort des idealen diskreten Tiefpassfilters benutzt. Wir benutzen dafür folgende gekürzte Definition.

Das Design unseres Filters ist mit der Fenstermethode erstellt, wobei wir das Rechteckfenster benutzen. Dies bedeutet, dass die Folge der diskreten Werte der Impulsantwort einfach auf die Länge des Filters geschnitten wird. Alle anderen Fenster erweisen sich als unbrauchbar. Im folgenden Plot ist das Ergebnis unseres Filters dargestellt. Der daraus resultierende Quellcode ist online auf dem SBC nutzbar. Das Resultat ist fast schon ideal zu nennen. Die zeitliche Verschiebung hält sich in Grenzen und die Amplituden entsprechen denen der Fourierdaten.

So weit so gut, die offline verarbeiteten Daten sehen gut aus, nur wie verhält sich die Software, wenn der Filter online auf den SBC implementiert wird? Die Antwort kann zum aktuellen Zeitpunkt noch nicht gegeben werden. Da der SBC mit jeder Datennahme den Roll – und Pitchwinkel filtern muss, was einer Iteration von 60 double-Werten entspricht, kann die Berechnung einen Einfluss auf die Verarbeitungszeit haben. Und dieses Ergebnis sehen wir auch in unseren Tests. Die Verarbeitungszeit und damit die Bearbeitungsfrequenz schwankt stark zwischen 100 und 400 Hz. Es ist noch nicht abschließend geklärt warum dies der Fall ist, aber es hat definitiv einen Einfluss auf unsere Software. Das ist der Punkt an dem wir derzeit stehen, stimmt die Performance des Filters mit dem überein, was wir erwarten oder müssen wir auf einen anderen Filter ausweichen?

Stay tuned at www.robobuam.de :-)

 

Robotik in Java.