Wie kann ich das Gyroskop und den Beschleunigungsmesser reduzieren, wenn ich PYB als Hauptsteuerung des ROS-Roboters verwenden möchte?
Artikelverzeichnis
MPU6050-Modul
MPU6050 ist ein sehr beliebter Raumbewegungssensor-Chip, der die aktuellen drei Beschleunigungskomponenten und drei Drehwinkelgeschwindigkeiten des Geräts erhalten kann. Aufgrund seiner geringen Größe, leistungsstarken Funktionen und hohen Präzision ist es nicht nur in der Industrie weit verbreitet, sondern auch ein Artefakt für Modellflugbegeisterte. Es wird in verschiedenen Flugzeugen installiert, um am blauen Himmel zu fliegen.
Der MPU6050-Chip wird mit einem Datenverarbeitungs-Submodul DMP geliefert, das über integrierte Filteralgorithmen verfügt. In vielen Anwendungen kann die von DMP ausgegebene Daten die Anforderungen gut erfüllen. Während der Verwendung müssen Sie nur den zu verwendenden Versatz subtrahieren. Wenn Sie höhere Anforderungen an die Genauigkeit haben, können Sie den oberen Computer verwenden oder den Kalman-Filter verwenden, um die Daten erneut zu optimieren.
Wenn Sie beim Kauf nur das Gyroskop und den Beschleunigungsmesser benötigen, können Sie das sechsachsige MPU6050-Modul GY-521 kaufen, wie in der Abbildung oben oder unten gezeigt. Es kostet nur etwa 4 Yuan für einen bestimmten Schatz. Es ist sehr billig, und Sie dürfen nicht das falsche Modell kaufen, sonst werden Sie ohne Tränen weinen, wenn Sie es verwenden. Möglicherweise können Sie die Bibliothek hier nicht direkt verwenden.
Hardware-Verbindung
Die Verdrahtungsrichtung und die MPU-Daten sind in der Abbildung dargestellt:
PYB | MPU6050 |
---|---|
VCC | VCC |
GND | GND |
SCL (X9, IIC1) | SCL |
SDL (X10, IIC1) | SDL |
Code
MPU6050-Bibliotheksdatei
mpu6050.py
import machine
class accel():
def __init__(self, i2c, addr=0x68):
self.iic = i2c
self.addr = addr
self.iic.start()
self.iic.writeto(self.addr, bytearray([107, 0]))
self.iic.stop()
def get_raw_values(self):
self.iic.start()
a = self.iic.readfrom_mem(self.addr, 0x3B, 14)
self.iic.stop()
return a
def get_ints(self):
b = self.get_raw_values()
c = []
for i in b:
c.append(i)
return c
def bytes_toint(self, firstbyte, secondbyte):
if not firstbyte & 0x80:
return firstbyte << 8 | secondbyte
return - (((firstbyte ^ 255) << 8) | (secondbyte ^ 255) + 1)
def get_values(self):
raw_ints = self.get_raw_values()
vals = {
}
vals["AcX"] = self.bytes_toint(raw_ints[0], raw_ints[1])
vals["AcY"] = self.bytes_toint(raw_ints[2], raw_ints[3])
vals["AcZ"] = self.bytes_toint(raw_ints[4], raw_ints[5])
vals["Tmp"] = self.bytes_toint(raw_ints[6], raw_ints[7]) / 340.00 + 36.53
vals["GyX"] = self.bytes_toint(raw_ints[8], raw_ints[9])
vals["GyY"] = self.bytes_toint(raw_ints[10], raw_ints[11])
vals["GyZ"] = self.bytes_toint(raw_ints[12], raw_ints[13])
return vals # returned in range of Int16
# -32768 to 32767
def val_test(self): # ONLY FOR TESTING! Also, fast reading sometimes crashes IIC
from time import sleep
while 1:
print(self.get_values())
sleep(0.05)
Hauptprogramm
from machine import I2C,Pin
i2c = I2C(scl=Pin("X9"), sda=Pin("X10"))
accel = mpu6050.accel(i2c)
accel_dict = accel.get_values()
print(accel_dict)
Betriebsergebnis:
Parameteranalyse
Die sechsachsige MPU6050 gibt alle Parameter aus:
- X-Achsen-Komponente des Beschleunigungsmessers:
ACC_X
- Y-Achsen-Komponente des Beschleunigungsmessers:
ACC_Y
- Die Z-Achsen-Komponente des Beschleunigungsmessers:
ACC_Z
- Aktuelle Temperatur:
TEMP
- Drehwinkelgeschwindigkeit um die X-Achse:
GYR_X
- Drehwinkelgeschwindigkeit um die Y-Achse:
GYR_Y
- Drehwinkelgeschwindigkeit um die Z-Achse:
GYR_Z
Das Koordinatensystem des MPU6050-Chips ist wie folgt definiert: Stellen Sie die Oberfläche des Chips auf Sie und drehen Sie den Oberflächentext in den richtigen Winkel. Zu diesem Zeitpunkt ist die Mitte des Chips der Ursprung, die horizontale rechte ist das X. Achse und die Vertikale nach oben ist Y Die Achse, die auf sich selbst zeigt, ist die Z-Achse.
Dreiachsiger Beschleunigungsmesser
Die dreiachsigen Komponenten ACC_X, ACC_Y und ACC_Z des Beschleunigungsmessers sind alle 16-Bit-Ganzzahlen mit Vorzeichen, die die Beschleunigung des Geräts in den drei Achsen darstellen. Wenn der Wert negativ ist, verläuft die Beschleunigung entlang der negativen Richtung der Koordinate Achse, und wenn der Wert positiv ist, ist es entlang der positiven Richtung.
Die drei Beschleunigungskomponenten sind in Vielfachen der Gravitationsbeschleunigung g angegeben, und der Beschleunigungsbereich, der ausgedrückt werden kann, dh die Vergrößerung kann gleichmäßig eingestellt werden, und es gibt 4 optionale Vergrößerungen: 2 g, 4 g, 8 g, 16 g. Das Ändern der Vergrößerung wird hier nicht vorgestellt. Wenn Sie Änderungen vornehmen möchten, schreiben Sie einfach die Flag-Bits, die verschiedenen Vergrößerungen entsprechen, in die entsprechenden Register. Die Standardeinstellung für die Vergrößerung ist 2 g . Wenn ACC_X
beispielsweise die Vergrößerung verwendet wird 2g
, beträgt ACC_X
der Mindestwert -32768 und der Höchstwert 32768. Wenn es ACC_X
32768 ist, beträgt die aktuelle Beschleunigung das Zweifache der Erdbeschleunigung entlang der positiven Richtung der X-Achse.
Die Beschleunigungsberechnungsformel ist in der Abbildung dargestellt:
Dreiachsiges Gyroskop
Winkelgeschwindigkeitskomponente um die X, Y und Z drei Koordinatendrehachsen GYR_X
, GYR_Y
und GYR_Z
sind 16-Bit - Ganzzahl mit Vorzeichen. Vom Ursprung bis zur Richtung der Rotationsachse bedeutet ein positiver Wert eine Drehung im Uhrzeigersinn und ein negativer Wert eine Drehung gegen den Uhrzeigersinn.
Die drei Winkelgeschwindigkeitskomponenten sind alle in "Grad / Sekunde" angegeben, dem Bereich der Winkelgeschwindigkeit, der ausgedrückt werden kann, dh die Vergrößerung kann gleichmäßig eingestellt werden, und es gibt 4 optionale Vergrößerungen: 250 Grad / Sekunde, 500 Grad / Sekunde 1000 Grad / Sekunde, 2000 Grad / Sekunde. Zum GYR_X
Beispiel, wenn die Vergrößerung auf 250 Grad eingestellt ist / Sekunde, bedeutet dies , dass , wenn GYR den positiven Maximalwert von 32768 in Anspruch nimmt, aktuelle Winkelgeschwindigkeit 250 Grad / Sekunde im Uhrzeigersinn ist, wenn es auf 500 Grad eingestellt ist / Sekunde, wenn es ist auf 32768 eingestellt, bedeutet dies, dass die aktuelle Winkelgeschwindigkeit 500 Grad / Sek. im Uhrzeigersinn beträgt. Je geringer die Vergrößerung, desto besser die Genauigkeit und je höher die Vergrößerung, desto größer der Bereich. Die Standardeinstellung der Vergrößerung ist 250 Grad / Sek . Die spezifische Modifikationsmethode wird hier nicht erläutert.
Die Umrechnungsformel der Winkelgeschwindigkeit lautet:
Datenverarbeitung und -realisierung
Die vom MPU6050-Chip bereitgestellten Daten werden mit starkem Rauschen gemischt, und der Datenhub kann 2% überschreiten, wenn der Chip den statischen Zustand verarbeitet. Zusätzlich zum Rauschen weisen die Daten auch ein Versatzphänomen auf, dh die Daten schwingen nicht um den statischen Betriebspunkt, sodass der Datenversatz zuerst kalibriert werden muss und dann das Rauschen durch den Filteralgorithmus beseitigt werden muss .
Kalibrierung
Die Kalibrierung ist eine relativ einfache Aufgabe, wir müssen nur den Mittelpunkt um die Schwungdaten finden. Nehmen wir als Beispiel GRY_X. Wenn sich der Chip in einem statischen Zustand befindet, sollte der Messwert theoretisch 0 sein, hat jedoch häufig einen Versatz. Beispielsweise lesen wir 10 Werte in Intervallen von 10 ms wie folgt:
-158.4, -172.9, -134.2, -155.1, -131.2, -146.8, -173.1, -188.6, -142.7, -179.5
Nehmen Sie den Durchschnitt dieser 10 Werte, dh der Versatz dieses Messwerts beträgt -158,25. Subtrahieren Sie nach Erhalt des Versatzes den Versatz von jedem Messwert, um den kalibrierten Messwert zu erhalten. Natürlich ist dieser Versatz nur ein geschätzter Wert, und ein genauerer Versatz kann nur durch Ausführen von Statistiken für eine große Datenmenge erhalten werden. Je größer die Datenmenge ist, desto genauer, aber die statistische Zeit ist langsamer. Im Allgemeinen kann die Kalibrierung bei jedem Systemstart durchgeführt werden. Daher sollten Sie einen Kompromiss zwischen Genauigkeit und Startzeit eingehen.
Die drei Winkelgeschwindigkeitsmesswerte GYR_X, GYR_Y und GYR_Z können alle durch statistische Mittelung erhalten werden, aber die drei Beschleunigungskomponenten können nicht so einfach durchgeführt werden, weil die Beschleunigung bei stationärem Chip nicht Null ist.
Der Versatz des Beschleunigungswerts ergibt sich aus zwei Aspekten: Zum einen ist der vom Chip gemessene Beschleunigungsvektor aufgrund der Messgenauigkeit des Chips nicht senkrecht zum Boden, zum anderen ist die Genauigkeit des Chips im gesamten System ( wie eine Drohne) ist begrenzt. Ja, das Koordinatensystem des Systems und des Chips ist schwierig, eine perfekte Übereinstimmung zu erzielen. Ersteres wird als Leseversatz bezeichnet, und letzteres wird als Winkelversatz bezeichnet. Da es eine nichtlineare Beziehung zwischen Messwert und Winkel gibt, müssen Sie, wenn Sie mit hoher Genauigkeit kalibrieren möchten, zuerst den Messversatz separat kalibrieren, dann den Chip im System fixieren und den Winkelversatz kalibrieren. Da jedoch professionelle Geräte zur Kalibrierung des Winkelversatzes erforderlich sind und für allgemeine Anwendungen die durch die zweistufige Kalibrierung erzielte Genauigkeitsverbesserung nicht groß ist, ist normalerweise nur die Lesekalibrierung erforderlich.
Kalibrierungsmethode lesen:
- Um den Einfluss des Versatzes des Messwerts so weit wie möglich zu vermeiden, befestigen Sie zuerst die MPU6050 fest im System und lassen Sie die beiden Koordinatensysteme so weit wie möglich zusammenfallen.
Zu diesem Zeitpunkt glauben wir, dass der theoretische Wert derACC_X
SummeACC_Y
der Chips 0 sein sollte undACC_Z
der theoretische Wert -16384 sein sollte (Standard 2g Vergrößerung).
DaACC_X
undACC_Y
der theoretische Wert 0 sein sollte, mit einer Winkelgeschwindigkeit ähnlich der Kalibrierung, versetzen zwei Kalibrierungsmittel den verfügbaren statistischen Mittelwert der Lesemittel.ACC_Z
Ein weiterer Schritt ist erforderlich, dh beim Zählen des Versatzes muss jedem Messwert 16384 hinzugefügt werden, und dann wird die statistische Mittelwertkalibrierung durchgeführt.
Kalman Filter
Bei Daten mit viel Rauschen ist der Effekt des Kalman-Filters zweifellos der beste. Wenn Sie die Details des Algorithmus nicht berücksichtigen möchten, können Sie direkt die Klaman Filter-Bibliothek von Arduino verwenden. In unserem Modell akzeptiert ein Kalman-Filter einen Achsenwinkelwert, einen Winkelgeschwindigkeitswert und ein Zeitinkrement und schätzt einen Winkelwert, der Rauschen eliminiert. Basierend auf dem aktuellen Winkelwert und dem in der vorherigen Runde geschätzten Winkelwert sowie der Intervallzeit zwischen den beiden Schätzrunden können wir auch die Winkelgeschwindigkeit ableiten, um Rauschen zu eliminieren.
Es wird empfohlen, die Kalman-Filterung auf dem Host-Computer zu implementieren. Micropython selbst bietet keine besonders gute Unterstützung für Matrixoperationen. Auf dem Host-Computer können Sie numpy verwenden, um schnelle Kalman-Filteroperationen zu implementieren.
Referenzartikel: