Berechnung von Neigung, Gieren und Rollen aus Mag-, Acc- und Gyro-Daten

19

Ich habe ein Arduino-Board mit einem Sensor für 9 Freiheitsgrade, anhand dessen ich die Neigung, das Gieren und das Rollen des Boards bestimmen muss.

Hier ist ein Beispiel für einen Datensatz des 9-DOF-Sensors:

Beschleunigungsmesser (m / s)

  • AccX = -5,85
  • AccY = 1,46
  • AccZ = 17,98

Gyroskop (U / min)

  • GyrX = 35,14
  • GyrY = -40,22
  • GyrZ = -9,86

Magnetometer (Gauß)

  • MagX = 0,18
  • MagY = -0,04
  • MagZ = -0,15

Wie kann ich aus diesen Daten Pitch, Yaw und Roll berechnen?

MaltDew
quelle
1
Grundprinzip: Anhand der Erkennung der Schwerkraft in Ihrem Beschleunigungsmesser wissen Sie, in welche Richtung Sie sich befinden. Anhand der Erfassung des Erdmagnetfelds in Ihrem Magnetometer wissen Sie, welcher Weg nach Norden führt. Auf dieser Basis und unter der Annahme, dass keine anderen signifikanten Beschleunigungen oder starken Magnetfelder vorliegen, können Sie Ihre eigene Haltung bestimmen.
Sonntag,
1
Gyroskopdaten liefern eine Rotationsrate, jedoch keine absolute Position. Es kann integriert werden, um Änderungen gegenüber einer bekannten Einstellung abzuschätzen. Dies ist jedoch in der Regel laut und driftgefährdet, wenn es nicht in Verbindung mit den anderen Sensoren verwendet wird.
Welf
1
Beziehen Sie sich auch auf Kalman-Filter, da die statischen Zahlen häufig verarbeitet werden müssen, um zuverlässige Schätzungen von Roll-Pitch und Gieren zu erhalten. Beachten Sie auch, dass die Position des Sensors wichtig ist (Sie müssen dies berücksichtigen).
Gürkan Çetin

Antworten:

15

Pitch, Roll und Yaw sind als Rotation um die X-, Y- und Z-Achse definiert. Unten als Bild zur Veranschaulichung der Definition.

Roll Pitch und Yaw

In einem früheren Projekt habe ich einen ADXL345-Beschleunigungsmesser von Analog Devices verwendet, um Roll und Nick zu berechnen. Nachfolgend sind die Gleichungen aufgeführt, die zur Berechnung von Wank- und Nickbewegungen verwendet werden. Ich habe einen Teil des Quellcodes zur öffentlichen Verwendung bereitgestellt.

accelerationX = (signed int)(((signed int)rawData_X) * 3.9);
accelerationY = (signed int)(((signed int)rawData_Y) * 3.9);
accelerationZ = (signed int)(((signed int)rawData_Z) * 3.9);
pitch = 180 * atan (accelerationX/sqrt(accelerationY*accelerationY + accelerationZ*accelerationZ))/M_PI;
roll = 180 * atan (accelerationY/sqrt(accelerationX*accelerationX + accelerationZ*accelerationZ))/M_PI;

Den vollständigen Quellcode finden Sie hier .

Beruhen Sie auf den obigen Definitionen

yaw = 180 * atan (accelerationZ/sqrt(accelerationX*accelerationX + accelerationZ*accelerationZ))/M_PI;

Hinweis: M_PI = 3.14159265358979323846 ist in math.h konstant definiert

Im Folgenden finden Sie einige Referenzen, einschließlich des Arduino-Basisquellcodes, die Ihnen möglicherweise helfen.


Verweise:

Mahendra Gunawardena
quelle
2
Gute Antwort, es wäre noch zu erwähnen, dass die Position und Ausrichtung des Sensors im Fahrzeug wichtig wäre und dass die Daten weiterverarbeitet werden müssen, um zuverlässige Ergebnisse zu erzielen. (gefiltert oder verschmolzen mit zuverlässigeren Niederfrequenzdaten wie GPS)
Gürkan Çetin
(@Zubair) "yaw = 180 * atan (BeschleunigungZ / sqrt (BeschleunigungXBeschleunigungX + BeschleunigungZBeschleunigungZ)) / M_PI;" Was ist das 'M_PI'?
Wasabi
@ Wasabi M_PI = 3.14159265358979323846. Es ist eine Konstante, die in der Bibliothek math.h definiert ist.
Mahendra Gunawardena
8

Meine längere Antwort im Folgenden geht daher davon aus, dass das Board beschleunigt wird und Sie in dieser Zeit immer noch in der Lage sein müssen, Ihre Neigung, Ihr Rollen und Ihr Gieren innerhalb kurzer Zeit zu messen. Wenn das Brett für alle Messungen stationär ist, funktioniert die Antwort von Mahendra Gunawardena perfekt für Sie. Wenn dies in ein Gerät wie einen Segway oder ein Modellflugzeug oder einen Multirotor oder irgendetwas geht, das sich bewegt, möchten Sie vielleicht weiterlesen. In diesem Beitrag wird erläutert, wie alle drei Sensoren mithilfe einer als Sensorfusion bezeichneten Methode verwendet werden. Mithilfe der Sensorfusion können Sie die Stärken der einzelnen Sensoren ermitteln und die Auswirkungen der Schwächen der einzelnen Sensoren minimieren.

Sensoreigenschaften und Hintergrund

Verstehen Sie zunächst, dass ein Beschleunigungsmesser alle Kräfte misst, die auf ihn einwirken, nicht nur die Schwerkraft. In einer perfekten Welt, in der sich der Beschleunigungsmesser in einer unbewegten Position ohne Vibrationen befindet, können Sie mithilfe einer einfachen Trigonometrie, wie Mahendra Gunawardenas Antwort zeigt, genau bestimmen, in welche Richtung er sich bewegt. Da ein Beschleunigungsmesser jedoch alle Kräfte aufnimmt, führen Vibrationen zu Geräuschen. Wenn das Board beschleunigt, können Sie nicht nur eine einfache Trigonometrie verwenden, da die vom Beschleunigungsmesser gemeldete Kraft nicht nur die Erdschwerkraft ist, sondern auch die Kraft, die Sie zum Beschleunigen veranlasst.

Ein Magnetometer ist einfacher als ein Beschleunigungsmesser. Bewegungen verursachen keine Probleme, aber Dinge wie Eisen und andere Magnete beeinträchtigen Ihre Leistung. Wenn die Quellen, die diese Störung verursachen, konstant sind, ist es nicht schwer damit umzugehen, aber wenn diese Quellen nicht konstant sind, wird es Tonnen von Geräuschen erzeugen, die problematisch zu beseitigen sind.

Von den drei Sensoren ist das Gyroskop wahrscheinlich der zuverlässigste und sie können normalerweise sehr gut die Drehzahl messen. Es wird nicht von Dingen wie Eisenquellen beeinflusst und Beschleunigungen haben im Grunde keinen Einfluss auf ihre Fähigkeit, die Drehzahl zu messen. Sie melden sehr gut die Geschwindigkeit, mit der sich das Gerät dreht. Da Sie jedoch einen absoluten Winkel suchen, müssen Sie die Geschwindigkeit integrieren, um die Position zu ermitteln. Auf diese Weise wird der Fehler der letzten Messung zum Fehler der neuen Messungen addiert, da die Integration im Grunde genommen eine Summe von Werten über einen Bereich ist, selbst wenn der Fehler für eine Messung nur 0,01 Grad pro Sekunde von Ihrer Position bei 100 Messungen abweicht kann um 1 Grad abweichen, bei 1000 Messungen können Sie um 10 Grad abweichen. Wenn Sie Hunderte von Messungen pro Sekunde durchführen, Sie können sehen, dass dies Probleme verursacht. Dies wird allgemein als Kreiseldrift bezeichnet.

Sensorfusion

Das Schöne daran, dass all diese Sensoren zusammenarbeiten, ist, dass Sie die Informationen von Beschleunigungsmesser und Magnetometer verwenden können, um die Kreiseldrift auszugleichen. Dies ermöglicht es Ihnen, die Genauigkeit und Geschwindigkeit des Kreisels ohne den fatalen Fehler der Kreiseldrift zu erreichen.

Die Daten dieser drei Sensoren können auf mehr als eine Weise kombiniert werden. Ich werde über die Verwendung eines Komplementärfilters sprechen, da ein Kalman-Filter und Kalman-Filter viel mehr Ressourcen auf eingebetteten Systemen verbrauchen. Oft ist ein komplementärer Filter gut genug, einfacher zu implementieren (vorausgesetzt, Sie verwenden keine vorgefertigte Bibliothek), und Sie können die Daten schneller verarbeiten.

Nun zum Prozess. Als erstes müssen Sie den Gyroskopausgang integrieren, um die Winkelgeschwindigkeit in die Winkelposition umzuwandeln. Sie müssen höchstwahrscheinlich auch einen Tiefpassfilter auf den Beschleunigungsmesser und den Magnetometer anwenden, um das Rauschen im Ausgang zu behandeln. Ein einfacher FIR-Filter wie der unten gezeigte funktioniert hier. Mit etwas Trigonometrie können Sie die Neigung und das Rollen mit dem Beschleunigungsmesser und das Gieren mit dem Magnetometer finden.

filteredData = (1-weight)*filteredData + weight*newData

Das Gewicht ist nur eine Konstante, die abhängig von der Lautstärke angepasst werden kann. Je höher die Lautstärke, desto geringer der Gewichtswert. Das Zusammenführen der Daten von den Sensoren kann nun durch die folgende Codezeile erfolgen.

fusedData = (1-weight)*gyroData + weight*accelMagData

Es ist anzumerken, dass die Daten ein Vektor des Nickens, Rollens und Gierens sind. Sie können auch nur drei Variablen anstelle von Arrays verwenden, wenn Sie möchten. Für diese Berechnung gibt der Kreisel eine Position in Grad in Nick, Roll und Gier an, der Magnetometer gibt einen Winkel für das Gieren an, während der Beschleunigungsmesser seine eigenen Zahlen für Nick und Roll angibt.

Wenn Sie noch mehr Informationen wünschen, können Sie "Sensor Fusion mit komplementärem Filter" googeln. Es gibt viele Artikel darüber.

Dave851
quelle
3

Aus den Gaspedalsensordaten können Sie nur Nick- und Rollbewegungen berechnen. Das folgende Dokument von Freescale erklärt mit vielen Informationen, was Sie brauchen:

AN3461 - Neigungserkennung mit einem dreiachsigen Beschleunigungsmesser

Basierend auf den Aussagen des Dokuments,

tanϕxyz=GpyGpz

tanθxyz=GpxGpysinϕ+Gpzcosϕ=GpxGpy2+Gpz2

das entspricht:

roll = atan2(accelerationY, accelerationZ)

pitch = atan2(-accelerationX, sqrt(accelerationY*accelerationY + accelerationZ*accelerationZ))

Das Ergebnis ist natürlich nur, wenn die Rotationen in einer bestimmten Reihenfolge (Rxyz) auftreten:

  1. ϕ
  2. θ
  3. Gieren Sie um den Winkel um die z-Achseψ

Rxyzψ

ekalyvio
quelle
1
Er, herzlich willkommen bei der Engineering SE! Diese Seite unterstützt Latex, sehen Sie, wie schön Ihre Antwort jetzt wurde. :-)
peterh - Wiedereinsetzung von Monica 10.06.18