AM 09: Inertialnavigation: Unterschied zwischen den Versionen
(31 dazwischenliegende Versionen von einem anderen Benutzer werden nicht angezeigt) | |||
Zeile 2: | Zeile 2: | ||
Dieser Wiki-Beitrag ist Teil eines Projektes, welches im Rahmen vom [[Fachpraktikum_Elektrotechnik_(WS_16/17)|Fachpraktikum Elektrotechnik]] im 5. Semester [http://www.hshl.de/mechatronik-bachelorstudiengang/ Mechatronik] absolviert wurde. Ziel des Beitrags ist es, eine nachhaltige Dokumentation zu schaffen, welche die Ergebnisse festhält und das weitere Arbeiten am Projekt ermöglicht. | Dieser Wiki-Beitrag ist Teil eines Projektes, welches im Rahmen vom [[Fachpraktikum_Elektrotechnik_(WS_16/17)|Fachpraktikum Elektrotechnik]] im 5. Semester [http://www.hshl.de/mechatronik-bachelorstudiengang/ Mechatronik] absolviert wurde. Ziel des Beitrags ist es, eine nachhaltige Dokumentation zu schaffen, welche die Ergebnisse festhält und das weitere Arbeiten am Projekt ermöglicht. | ||
Autoren: | Autoren: [[Benutzer:Hanno_Kerber| Hanno Kerber]], [[Benutzer:Jan_Auf_der_Landwehr| Jan Auf der Landwehr]] | ||
Betreuer: [[Benutzer:Ulrich_Schneider| Prof. Schneider]] | Betreuer: [[Benutzer:Ulrich_Schneider| Prof. Schneider]] | ||
Zeile 16: | Zeile 16: | ||
*Aufbau und Integration der Inertialsensorik (Kompass, 3-Achs-Gyros, 3-Achs-Beschleunigungsmesser) | *Aufbau und Integration der Inertialsensorik (Kompass, 3-Achs-Gyros, 3-Achs-Beschleunigungsmesser) | ||
*Kalibrierung der Sensoren | *Kalibrierung der Sensoren | ||
*Inertialnavigation ( | *Inertialnavigation (Sensordatenfusion) anhand der IMU | ||
*Darstellung der Ergebnisse im Vergleich zur Referenz | *Darstellung der Ergebnisse im Vergleich zur Referenz | ||
*Erstellen Sie ein faszinierendes Video, welches die Funktion visualisiert. | *Erstellen Sie ein faszinierendes Video, welches die Funktion visualisiert. | ||
Zeile 34: | Zeile 34: | ||
1. Erstellung einer BOM und Beschaffung der IMU | 1. Erstellung einer BOM und Beschaffung der IMU | ||
2. Löten der Anschlusspins an die IMU | 2. Löten der Anschlusspins an die IMU | ||
3. Einlesen in das Datasheet des Sensors | 3. Einlesen in das Datasheet des Sensors | ||
4. Funktionstest mit einem Arduino und passender Library | 4. Funktionstest mit einem Arduino und passender Library | ||
5. Fehlersuche und -behebung mithilfe grafischer Ausgabe der Sensordaten | 5. Fehlersuche und -behebung mithilfe grafischer Ausgabe der Sensordaten | ||
6. Anwenden von Filtern (Tiefpassfilter, Kalman) und Kalibrierungsalgorithmen zur Verbesserung der Daten | 6. Anwenden von Filtern (Tiefpassfilter, Kalman) und Kalibrierungsalgorithmen zur Verbesserung der Daten | ||
7. 3D-Darstellung der Bewegung eines Testkörpers in Matlab | 7. 3D-Darstellung der Bewegung eines Testkörpers in Matlab | ||
== Verwendete Bauteile == | == Verwendete Bauteile== | ||
Die inertiale Messeinheit (IMU) GY-85 kombiniert mehrere Inertialsensoren auf einer Platine. Enthalten sind: | Die '''inertiale Messeinheit''' (IMU) GY-85 kombiniert mehrere Inertialsensoren auf einer Platine. Enthalten sind: | ||
[[Datei:BeschleunigungssensorModell.JPG|450px|thumb|Abbildung 1: Beschleunigungssensor]] | |||
Zeile 53: | Zeile 60: | ||
- ITG3205 (3-Achsen-Drehratensensor (Gyro) und integrierter Temperatursensor) | - ITG3205 (3-Achsen-Drehratensensor (Gyro) und integrierter Temperatursensor) | ||
= | |||
'''HMC5883L (3-Achsen Digitalkompass)''' | |||
Der Kompasssensor stellt Werte für 3-Achsen bereit. Bei einer ebenen Ausrichtung sind nur X- und Y-Achse nötig, um die Abweichung von Norden zu berechnen. Ein 0° Kurs kennzeichnet die Ausrichtung nach Norden. Der Kompasssensor gibt den Kurs absolut an, ist jedoch störungsbehaftet, besonders wenn sich Ferromagnetika oder stromdurchflossene Leiter (v.a. Elektromotoren) in nahem Radius um den Sensor befinden. Der Sensor sollte mit einem minimalen Abstand von 30 cm zu Motoren angebracht werden. | |||
'''ITG3205 (3-Achsen-Drehratensensor, Gyrosensor)''' | |||
Der Gyrosensor misst Änderungen der Lage des Sensors im Raum und kann Winkelveränderungen um alle drei Drehachsen (3-Achsen Gyrosensor) detektieren. Seine Funktionalität beruht auf der Massenträgheit. Moderne MEMS-Gyrosensoren nutzen meist den piezoelektrischen Effekt, um ein oder mehrere Piezo-Elemente in Schwingung zu versetzen. Wird die Lagerung des Elementes nun in eine andere Richtung ausgerichtet, verformt dieses sich kurzzeitig, da es aufgrund der Trägheit seiner Masse zunächst in seiner vorherigen Schwingungsposition verbleibt. Der piezoelektrische Effekt findet nun umgekehrt statt und es wird eine Spannung induziert. Sie bildet das Ausgangssignal des Sensors und ist proportional zu dessen Winkelgeschwindigkeit. Die Werte des Gyrosensor kurzzeitig sehr genau, driften allerdings über die Zeit. | |||
'''ADXL345 (3-Achsen-Beschleunigungssensor)''' | |||
Der Beschleunigungssensor misst (Überraschung!) Beschleunigungen. Abbildung 1 zeigt das Modell eines solchen Sensors. Es handelt sich bei diesem Sensor um ein mikro-elektro-mechanische System (MEMS). Innerhalb des Sensors befindet sich eine kleine Testmasse, die allseitig von Federn gelagert ist. Bei Beschleunigungen wirkt durch die Trägheit der Testmasse eine Kraft auf eine oder mehrere der Federn. Die Auslenkung der Federn wiederum kann gemessen werden. Im Ruhezustand wird als einzige Beschleunigung die Erdbeschleunigung gemessen. Da diese immer Richtung Erdmittelpunkt gerichtet ist, kann der Beschleunigungssensor gut als Referenz und zu Korrektionen der aktuellen Ausrichtung genutzt werden. Die Werte driften nicht über Zeit, weisen allerdings ein hohes Rauschen auf und sind deshalb zur direkten Winkelbestimmung weniger geeignet die des Gyrosensors. | |||
Soll die IMU mit dem vorhandenen ArduMower Software verwendet werden, ist es notwenig das Gyroskop GY-80 zu bestellen. Nur dieses Gyroskop kann (ohne eigene Veränderungen des Codes) mit der Open-Source Software genutzt werden. (Stand: 12.01.2017) | |||
= Einleitung= | |||
'''Inertialnavigation mithilfe der GY-85 IMU ''' | '''Inertialnavigation mithilfe der GY-85 IMU ''' | ||
[[Datei:Fusion.png| | [[Datei:Fusion.png|350px|thumb|Abbildung 2: Sensordatenfusion]] | ||
Zeile 63: | Zeile 85: | ||
''' | ''Erste Schritte'' | ||
Um die Kommunikation zu den einzelnen Sensoren der inertialen Messeinheit zu verstehen, kann diese zunächst testweise durch kleine Arduino Sketches vorgenommen werden. Dazu wird ein kleines Testprogramm geschrieben. Mithilfe der Standardbibliothek [https://www.arduino.cc/en/Reference/Wire Wire] sind I2C Verbindungen zwischen Arduino und Sensoren simple. So können beispielsweise die Einstellungs Register der Sensoren beschrieben oder die Messdaten aus den Daten-Registern gelesen werden. Ein Bespiel hierfür bietet das Wire_Accel_Connect.ino Programm. Die I2C-Adressen der Sensoren können den Datasheets entnommen werden, wo weiterhin auch die Adressierung der einzelnen Register und deren Bedeutungen zu finden sind. Nach Verbindung der IMU mit dem Arduino-Board können die Adressen auch mit dem [https://svn.hshl.de/svn/Elektrotechnik_Fachpraktikum/trunk/Projekte/ArduMower/Fachbereich%205%20Software/5.4%20Inertialnavigation/i2c_scanner/ I2C Scanner] ausgelesen werden | |||
Nach erfolgreichem Testen der einzelnen Sensoren werden für die weitere Verwendung der Sensoren Bibliotheken verwendet, die die Handhabung erleichtern. | |||
Die Libraries für die einzelnen Sensoren sind [https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/ hier] erhältlich . | |||
Die Bibliothek [http://playground.arduino.cc/Code/Filters Filters] übernimmt die Implementierung von Tiefpassfiltern für die Glättung der Sensorwerte. | |||
Die verschiedenen Quellcodes sind hier zu finden: [https://svn.hshl.de/svn/Elektrotechnik_Fachpraktikum/trunk/Projekte/ArduMower/Fachbereich%205%20Software Link für das SVN-Projektarchiv Inertialnavigation] | |||
= Entwicklung der Software= | |||
[[Datei:Rollpitchyawplain_deutsch.png|350px|thumb|Abbildung 3: Rotationsachsen]] | |||
'''Beschreibung des Hauptprogramms [https://svn.hshl.de/svn/Elektrotechnik_Fachpraktikum/trunk/Projekte/ArduMower/Fachbereich%205%20Software/5.4%20Inertialnavigation/Inertialnavigation/Inertialnavigation.ino Inertialnavigation.ino]''' | |||
Initialisierung | |||
''Beispiel Beschleunigungssensor:'' | |||
ADXL345 accel; // erstelle Accelerometer Objekt | |||
int16_t ax, ay, az; // Variablen zum Speichern der Accelerometer-Daten | |||
FilterOnePole lowPass_ax(LOWPASS, 0.5); // Erstellen eines Tiefpassfilter Objekts, 0.5 Hz Filterfrequenz (für alle drei Achsen separat) | |||
accel.initialize(); // Initialisieren des Accelerometer Sensors in der Setup Routine | |||
Nachdem Objekte für die einzelnen Sensor-Bibliotheken angelegt wurden, werden diese innerhalb der Setup-Routine initialisiert. Hier können auch Einstellungen an den Sensoren vorgenommen werden. Innerhalb der Main/Loop Routine werden nun kontinuierlich Sensorrohdaten eingelesen und anschließend mit einen Tiefpassfilter gefiltert. Es folgen Berechnungen um die Rohdaten in Winkel umzurechnen. | |||
''Beispiel Gyrosensor:'' | |||
float gyroDegreesZ = float(lowPass_gz.output()) * float(timediff)/1000.0; // Integration der Winkelgeschwindigkeit zur Berechnung der Winkeldifferenz; // timediff Umwandlung von Millisekunden zu Sekunden | |||
gyroHeadingZ += (gyroDegreesZ + gyroHeadingCorrectionZ); // Berechnung des neuen Winkels als Summe des alten Winkels, der WInkeldifferenz und der Driftkorrektur | |||
if(gyroHeadingZ < -180) // Überlaufabfrage, damit sich der Winkel stets zwischen -180° und +180° befindet | |||
gyroHeadingZ += 360; | |||
else if(gyroHeadingZ > 180) // Überlaufabfrage, damit sich der Winkel stets zwischen -180° und +180° befindet | |||
gyroHeadingZ -= 360; | |||
[[Datei:InertialnavigationMatlabModel.PNG|350px|thumb|Abbildung 4: Visualisierung der Sensorlage]] | |||
Der Gyrosensor misst die Winkelgeschwindigkeit. Soll aus dieser der relative Winkel berechnet werden, müssen die Sensordaten integriert werden. Im Programm wird dafür die Zeit in Millisekunden pro Schleifendurchlauf gemessen und in der Variable timediff gespeichert. Für die Berechnung des Winkels wird nun in jedem Durchlauf der aktuelle Wert mit der Zeitdifferenz multipliziert und zu dem aktuellen Winkel addiert. In einer Kalibrierungsphase wird zudem nach dem Start des Programms eine Driftkorrektur berechnet und in gespeichert, um den Winkelwert zu verbessern. | |||
Am Schluss der Hauptschleife werden die Winkel für Gieren, Nicken und Rollen (siehe Abbildung 3) über die serielle Verbindung vom Arduino-Board an den PC gesendet und können dann z.B. durch das [https://svn.hshl.de/svn/Elektrotechnik_Fachpraktikum/trunk/Projekte/ArduMower/Fachbereich%205%20Software/5.4%20Inertialnavigation/Matlab-Model/IMU3dModel/ Matlab-Script IMU3dModel.m] visualisiert werden oder direkt für die Weiterverarbeitung innerhalb der ArduMower oder ArduMower Mini Software verwendet werden. | |||
''' | '''Beschreibung des Matlab-Programms [https://svn.hshl.de/svn/Elektrotechnik_Fachpraktikum/trunk/Projekte/ArduMower/Fachbereich%205%20Software/5.4%20Inertialnavigation/Matlab-Model/IMU3dModel/ IMU3dModel.m]''' | ||
Das Matlab-Script IMU3dModel.m stellt eine serielle USB Kommunikation zu einem Arduino her und liest kontinuierlich Daten ein. Es speichert dabei die Lagewinkel für die Längs-, Quer- und Hochachse wie sie vom Arduino mit dem Inertialnavigation.ino Programm gesendet werden. | |||
Innerhalb von Matlab wird nun eine 3d Form gezeichnet, deren Eckpunkte durch Rotationsmatrizen für X-, Y- und Z-Achse und entsprechend den eingelesenen Nick-, Roll- und Gierwinkel rotiert werden. Auch der Status der vom Arduino durchgeführten Kalibrierung wird von Matlab eingelesen und während des Kalibriervorgangs erscheint das 3d-Modell in einer anderen Farbe. | |||
Das Matlab-Programm [https://svn.hshl.de/svn/Elektrotechnik_Fachpraktikum/trunk/Projekte/ArduMower/Fachbereich%205%20Software/5.4%20Inertialnavigation/Matlab-Model/KalmanFilter/ KalmanFilter] ist eine Weiterentwicklung des Scripts zur Visualisierung und implementiert ein Kalman-Filter für die verbesserte Genauigkeit der Lagewinkel. Entwickelt wurde dieses Script unter Verwendung von 'Kim, P.: Kalman-Filter für Einsteiger, mit Matlab Beispielen.'. Es befindet sich noch in der Entwicklung, zeigt allerdings schon die Vorteile der Sensordatenfusion. | |||
= Zusammenfassung = | |||
Durch die Anordnung von drei Lagesensoren mit verschiedener Messtechnik auf einer sehr kleinen Fläche und zu einem erschwinglichen Preis bieten sich viele Möglichkeiten und Einsatzgebiete für IMUs. Da alle drei Sensoren den gleichen Bewegungen ausgesetzt werden, ist die Fusion der Sensordaten eine gute Möglichkeit, die Genauigkeit der Messdaten zu erhöhen. | |||
== Ausblick == | == Ausblick == | ||
Mit der erfolgreichen Inbetriebnahme der inertialen Messeinheit und der verlässlichen Bestimmung von Lagedaten kann die Technik der Inertialnavigation nun als wichtige Sensoreinheit Einsatz auf dem ArduMower und dem ArduMower Mini finden. Unabdingbar ist er beispielsweise für die eigenständige Navigation des Roboters und für sichheitsrelevante Features wie der Abschaltautomatik beim Anheben oder Umkippen des Mähroboters. | |||
= Weiterführende Links = | = Weiterführende Links = | ||
*[https://svn.hshl.de/svn/Elektrotechnik_Fachpraktikum/trunk/Projekte/ArduMower/Fachbereich%205%20Software Link für das SVN-Projektarchiv Inertialnavigation] | |||
*[http://wiki.ardumower.de/index.php?title=Deutsche_Version Ardumower Wiki] | *[http://wiki.ardumower.de/index.php?title=Deutsche_Version Ardumower Wiki] | ||
*[https://www.marotronics.de/9-Achsen-IMU-Sensor-GY-801-L3G4200D-ADXL345-HMC5883L-BMP180 Web-Shop: IMU] | *[https://www.marotronics.de/9-Achsen-IMU-Sensor-GY-801-L3G4200D-ADXL345-HMC5883L-BMP180 Web-Shop: IMU] |
Aktuelle Version vom 15. Januar 2017, 19:14 Uhr
Dieser Wiki-Beitrag ist Teil eines Projektes, welches im Rahmen vom Fachpraktikum Elektrotechnik im 5. Semester Mechatronik absolviert wurde. Ziel des Beitrags ist es, eine nachhaltige Dokumentation zu schaffen, welche die Ergebnisse festhält und das weitere Arbeiten am Projekt ermöglicht.
Autoren: Hanno Kerber, Jan Auf der Landwehr
Betreuer: Prof. Schneider
Aufgabe
Integration einer IMU
Erwartungen an die Projektlösung
- Einarbeitung in die bestehenden Ardumowers-Unterlagen
- Planung und Beschaffung der Bauteile
- Aufbau und Integration der Inertialsensorik (Kompass, 3-Achs-Gyros, 3-Achs-Beschleunigungsmesser)
- Kalibrierung der Sensoren
- Inertialnavigation (Sensordatenfusion) anhand der IMU
- Darstellung der Ergebnisse im Vergleich zur Referenz
- Erstellen Sie ein faszinierendes Video, welches die Funktion visualisiert.
- Test und wiss. Dokumentation
Schwierigkeitsgrad
- Mechanik: *
- Elektrotechnik: *
- Informatik: **
Einleitung
Das Ardumowerprojekt des 5. Semesters Mechatronik in Kooperation mit dem Masterstudiengang Business and System Design Engineering an der Hochschule Hamm-Lippstadt hat zum Ziel, einen voll funktionsfähigen autonomen Rasenmähroboter in Betrieb zu nehmen. Dazu werden die notwendigen Aufgaben für die Erfüllung des Projektes auf einzelne Gruppen aufgeteilt. Die Gruppe Inertialnavigation beschäftigt sich mit der Inbetriebnahme einer IMU (Inertial Measurement Unit), die eine Kombination aus drei verschiedenen Sensoren auf einer Platine darstellt. Es sind dabei ein Magnetsensor (Kompass), ein Beschleunigungssensor und ein Gierratensensor vorhanden. Damit lässt sich die Lage des Mähroboters bestimmen, nachdem die IMU verbaut wurde.
Projektdurchführung
Projektplan
1. Erstellung einer BOM und Beschaffung der IMU
2. Löten der Anschlusspins an die IMU
3. Einlesen in das Datasheet des Sensors
4. Funktionstest mit einem Arduino und passender Library
5. Fehlersuche und -behebung mithilfe grafischer Ausgabe der Sensordaten
6. Anwenden von Filtern (Tiefpassfilter, Kalman) und Kalibrierungsalgorithmen zur Verbesserung der Daten
7. 3D-Darstellung der Bewegung eines Testkörpers in Matlab
Verwendete Bauteile
Die inertiale Messeinheit (IMU) GY-85 kombiniert mehrere Inertialsensoren auf einer Platine. Enthalten sind:
- ADXL345 (3-Achsen-Beschleunigungssensor)
- HMC5883L (3-Achsen Digitalkompass)
- ITG3205 (3-Achsen-Drehratensensor (Gyro) und integrierter Temperatursensor)
HMC5883L (3-Achsen Digitalkompass)
Der Kompasssensor stellt Werte für 3-Achsen bereit. Bei einer ebenen Ausrichtung sind nur X- und Y-Achse nötig, um die Abweichung von Norden zu berechnen. Ein 0° Kurs kennzeichnet die Ausrichtung nach Norden. Der Kompasssensor gibt den Kurs absolut an, ist jedoch störungsbehaftet, besonders wenn sich Ferromagnetika oder stromdurchflossene Leiter (v.a. Elektromotoren) in nahem Radius um den Sensor befinden. Der Sensor sollte mit einem minimalen Abstand von 30 cm zu Motoren angebracht werden.
ITG3205 (3-Achsen-Drehratensensor, Gyrosensor)
Der Gyrosensor misst Änderungen der Lage des Sensors im Raum und kann Winkelveränderungen um alle drei Drehachsen (3-Achsen Gyrosensor) detektieren. Seine Funktionalität beruht auf der Massenträgheit. Moderne MEMS-Gyrosensoren nutzen meist den piezoelektrischen Effekt, um ein oder mehrere Piezo-Elemente in Schwingung zu versetzen. Wird die Lagerung des Elementes nun in eine andere Richtung ausgerichtet, verformt dieses sich kurzzeitig, da es aufgrund der Trägheit seiner Masse zunächst in seiner vorherigen Schwingungsposition verbleibt. Der piezoelektrische Effekt findet nun umgekehrt statt und es wird eine Spannung induziert. Sie bildet das Ausgangssignal des Sensors und ist proportional zu dessen Winkelgeschwindigkeit. Die Werte des Gyrosensor kurzzeitig sehr genau, driften allerdings über die Zeit.
ADXL345 (3-Achsen-Beschleunigungssensor)
Der Beschleunigungssensor misst (Überraschung!) Beschleunigungen. Abbildung 1 zeigt das Modell eines solchen Sensors. Es handelt sich bei diesem Sensor um ein mikro-elektro-mechanische System (MEMS). Innerhalb des Sensors befindet sich eine kleine Testmasse, die allseitig von Federn gelagert ist. Bei Beschleunigungen wirkt durch die Trägheit der Testmasse eine Kraft auf eine oder mehrere der Federn. Die Auslenkung der Federn wiederum kann gemessen werden. Im Ruhezustand wird als einzige Beschleunigung die Erdbeschleunigung gemessen. Da diese immer Richtung Erdmittelpunkt gerichtet ist, kann der Beschleunigungssensor gut als Referenz und zu Korrektionen der aktuellen Ausrichtung genutzt werden. Die Werte driften nicht über Zeit, weisen allerdings ein hohes Rauschen auf und sind deshalb zur direkten Winkelbestimmung weniger geeignet die des Gyrosensors.
Soll die IMU mit dem vorhandenen ArduMower Software verwendet werden, ist es notwenig das Gyroskop GY-80 zu bestellen. Nur dieses Gyroskop kann (ohne eigene Veränderungen des Codes) mit der Open-Source Software genutzt werden. (Stand: 12.01.2017)
Einleitung
Inertialnavigation mithilfe der GY-85 IMU
Die Datenübertragung von Sensor zu Mikrocontroller findet über eine I²C-Kommunikation statt. Die Sensoren weisen verschiedene Schwächen auf, die bei der Nutzung als Navigationssensoren zu berücksichtigen sind.
Wie in Abbildung 1 ersichtlich, soll durch die Fusionierung der drei Sensordaten ein genauerer Kurs ermittelt werden, als würde nur einer der Sensoren benutzt werden. Dabei werden auf kurze Laufzeiten die Daten des Gyrosensors genutzt. In wiederkehrenden Abständen werden diese Werte durch Daten des Beschleunigungssensors korrigiert. Zusätzlich findet der Kompasssensor Anwendung zur Korrektion dieser Werte.
Erste Schritte
Um die Kommunikation zu den einzelnen Sensoren der inertialen Messeinheit zu verstehen, kann diese zunächst testweise durch kleine Arduino Sketches vorgenommen werden. Dazu wird ein kleines Testprogramm geschrieben. Mithilfe der Standardbibliothek Wire sind I2C Verbindungen zwischen Arduino und Sensoren simple. So können beispielsweise die Einstellungs Register der Sensoren beschrieben oder die Messdaten aus den Daten-Registern gelesen werden. Ein Bespiel hierfür bietet das Wire_Accel_Connect.ino Programm. Die I2C-Adressen der Sensoren können den Datasheets entnommen werden, wo weiterhin auch die Adressierung der einzelnen Register und deren Bedeutungen zu finden sind. Nach Verbindung der IMU mit dem Arduino-Board können die Adressen auch mit dem I2C Scanner ausgelesen werden
Nach erfolgreichem Testen der einzelnen Sensoren werden für die weitere Verwendung der Sensoren Bibliotheken verwendet, die die Handhabung erleichtern.
Die Libraries für die einzelnen Sensoren sind hier erhältlich .
Die Bibliothek Filters übernimmt die Implementierung von Tiefpassfiltern für die Glättung der Sensorwerte.
Die verschiedenen Quellcodes sind hier zu finden: Link für das SVN-Projektarchiv Inertialnavigation
Entwicklung der Software
Beschreibung des Hauptprogramms Inertialnavigation.ino
Initialisierung
Beispiel Beschleunigungssensor:
ADXL345 accel; // erstelle Accelerometer Objekt int16_t ax, ay, az; // Variablen zum Speichern der Accelerometer-Daten FilterOnePole lowPass_ax(LOWPASS, 0.5); // Erstellen eines Tiefpassfilter Objekts, 0.5 Hz Filterfrequenz (für alle drei Achsen separat) accel.initialize(); // Initialisieren des Accelerometer Sensors in der Setup Routine
Nachdem Objekte für die einzelnen Sensor-Bibliotheken angelegt wurden, werden diese innerhalb der Setup-Routine initialisiert. Hier können auch Einstellungen an den Sensoren vorgenommen werden. Innerhalb der Main/Loop Routine werden nun kontinuierlich Sensorrohdaten eingelesen und anschließend mit einen Tiefpassfilter gefiltert. Es folgen Berechnungen um die Rohdaten in Winkel umzurechnen.
Beispiel Gyrosensor:
float gyroDegreesZ = float(lowPass_gz.output()) * float(timediff)/1000.0; // Integration der Winkelgeschwindigkeit zur Berechnung der Winkeldifferenz; // timediff Umwandlung von Millisekunden zu Sekunden gyroHeadingZ += (gyroDegreesZ + gyroHeadingCorrectionZ); // Berechnung des neuen Winkels als Summe des alten Winkels, der WInkeldifferenz und der Driftkorrektur if(gyroHeadingZ < -180) // Überlaufabfrage, damit sich der Winkel stets zwischen -180° und +180° befindet gyroHeadingZ += 360; else if(gyroHeadingZ > 180) // Überlaufabfrage, damit sich der Winkel stets zwischen -180° und +180° befindet gyroHeadingZ -= 360;
Der Gyrosensor misst die Winkelgeschwindigkeit. Soll aus dieser der relative Winkel berechnet werden, müssen die Sensordaten integriert werden. Im Programm wird dafür die Zeit in Millisekunden pro Schleifendurchlauf gemessen und in der Variable timediff gespeichert. Für die Berechnung des Winkels wird nun in jedem Durchlauf der aktuelle Wert mit der Zeitdifferenz multipliziert und zu dem aktuellen Winkel addiert. In einer Kalibrierungsphase wird zudem nach dem Start des Programms eine Driftkorrektur berechnet und in gespeichert, um den Winkelwert zu verbessern.
Am Schluss der Hauptschleife werden die Winkel für Gieren, Nicken und Rollen (siehe Abbildung 3) über die serielle Verbindung vom Arduino-Board an den PC gesendet und können dann z.B. durch das Matlab-Script IMU3dModel.m visualisiert werden oder direkt für die Weiterverarbeitung innerhalb der ArduMower oder ArduMower Mini Software verwendet werden.
Beschreibung des Matlab-Programms IMU3dModel.m
Das Matlab-Script IMU3dModel.m stellt eine serielle USB Kommunikation zu einem Arduino her und liest kontinuierlich Daten ein. Es speichert dabei die Lagewinkel für die Längs-, Quer- und Hochachse wie sie vom Arduino mit dem Inertialnavigation.ino Programm gesendet werden. Innerhalb von Matlab wird nun eine 3d Form gezeichnet, deren Eckpunkte durch Rotationsmatrizen für X-, Y- und Z-Achse und entsprechend den eingelesenen Nick-, Roll- und Gierwinkel rotiert werden. Auch der Status der vom Arduino durchgeführten Kalibrierung wird von Matlab eingelesen und während des Kalibriervorgangs erscheint das 3d-Modell in einer anderen Farbe.
Das Matlab-Programm KalmanFilter ist eine Weiterentwicklung des Scripts zur Visualisierung und implementiert ein Kalman-Filter für die verbesserte Genauigkeit der Lagewinkel. Entwickelt wurde dieses Script unter Verwendung von 'Kim, P.: Kalman-Filter für Einsteiger, mit Matlab Beispielen.'. Es befindet sich noch in der Entwicklung, zeigt allerdings schon die Vorteile der Sensordatenfusion.
Zusammenfassung
Durch die Anordnung von drei Lagesensoren mit verschiedener Messtechnik auf einer sehr kleinen Fläche und zu einem erschwinglichen Preis bieten sich viele Möglichkeiten und Einsatzgebiete für IMUs. Da alle drei Sensoren den gleichen Bewegungen ausgesetzt werden, ist die Fusion der Sensordaten eine gute Möglichkeit, die Genauigkeit der Messdaten zu erhöhen.
Ausblick
Mit der erfolgreichen Inbetriebnahme der inertialen Messeinheit und der verlässlichen Bestimmung von Lagedaten kann die Technik der Inertialnavigation nun als wichtige Sensoreinheit Einsatz auf dem ArduMower und dem ArduMower Mini finden. Unabdingbar ist er beispielsweise für die eigenständige Navigation des Roboters und für sichheitsrelevante Features wie der Abschaltautomatik beim Anheben oder Umkippen des Mähroboters.
Weiterführende Links
- Link für das SVN-Projektarchiv Inertialnavigation
- Ardumower Wiki
- Web-Shop: IMU
- Wiki: IMU
- Global Navigation Satellite Systems, Inertial Navigation, and Integration, 3e
- Matlab File Exchange: Visual Inertial Odometry
- Simulink Blocksatz: Three-Axis Inertial Measurement Unit
- Simulink Blocksatz: Three-Axis Gyroscope
- Simulink Blocksatz: Three-Axis Accelerometer
- MATLAB Examples: Designing a Guidance System in MATLAB and Simulink
- MathWorks: Quadcopter Project
- [Efficient and Robust Pose Estimation Based on Inertial and Visual Sensing https://mediatum.ub.tum.de/doc/1092914/document.pdf]
- Inertiasl-Visual Odometry on mobile Devices
- Live 3D Reconstruction on Mobile Phones
Unterlagen
Literatur
- Wendel, J.': Integrierte Navigationssysteme: Sensordatenfusion, GPS und inertiale Navigation. München : Oldenbourg Verlag, 2. Auflage 2011. ISBN: 9783486704396 eBook
YouTube-Video
→ zurück zum Hauptartikel: Fachpraktikum Elektrotechnik (WS 16/17)