Ik reposted gewoon, dus zelfs meer gebruik van het krijg. De code kan hier worden gevonden:
https://github.com/TKJElectronics/example-sketch-for-IMU-including-Kalman-filter
Hallo iedereen
Onlangs kocht ik dit analoge 6DOF (zes graden van vrijheid) IMU board (http://www.sparkfun.com/products/10010) van watterott.com. Het maakt gebruik van drie gyros en drie versnellingsmeters voor het berekenen van hoeken in drie dimensies.
Ik keek een tijdje voor wat code online en hoe te verbinden met hen. Na vele uren van onderzoek volgde ik de nauwkeurige meting af van hoeken in twee richtingen te maken. Ik besloot te schrijven een korte handleiding voor collega elektronische liefhebbers.
Het belangrijkste doel van deze gids is aan anderen te leren hoe je sommige nuttige gegevens uit hun IMU of gewoon een gyro of versnellingsmeter. De code voor de Arduino kan worden gevonden op github: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter. Het moet vrij eenvoudig te implementeren van mijn code aan uw eigen sensor. Ik zal niet alle details over de theorie achter beschrijven, in plaats daarvan kunt u kijken naar de bronnen voor meer info.
Voordat u begint moet u verbinden de IMU als volgt:
Acc_Gyro Arduino
3,3 v <> — 3, 3V
<> — GND GND
Gx4 X <> — AN0
Gx4 Y <> — AN1
Gx4 Z <> — AN2
ACC X <> — AN3
ACC Y <> — AN4
ACC Z <> — AN5
Ook verbinden met 3,3 v de AREF pin op de Arduino voor meer nauwkeurigheid.
Het is zeer belangrijk dat u niet de sensor aan 5V sluit-dit de sensor vernietigen zal.
Nu u bent klaar voor het lezen van bepaalde gegevens van de sensor.
Om te communiceren met de sensor is eenvoudig:
De gyro meet graden per seconde, terwijl de versnellingsmeter versnelling (g's) in drie dimensies maatregelen. Beide uitgangen de metingen als een analoog signaal.
Om deze te krijgen vertaald in graden, die u hoeft te doen sommige codering:
De gyro
Eerst moet u quids (een getal tussen 0-1023) worden omgezet in iets nuttigs (dit is voor een ADC met een 10 bit resolutie, dit moet bijvoorbeeld 4095 (2 ^ 12-1 = 4095) voor 12 bit ADC). Om dit te doen gebruik ik gewoon deze eenvoudige vergelijking:
gyroRate = (gyroAdc-gyroZero) / gevoeligheid - waar gyroAdc de readed waarde van onze sensor, gyroZero is de waarde wanneer het stationaire (dit wordt gedaan in de code - kijk in de sectie "Installatie") terwijl gevoeligheid is de gevoeligheid gevonden in het gegevensblad, maar vertaald in quids.
Als je in de gegevensbladen van de twee gyros (http://www.sparkfun.com/datasheets/Sensors/IMU/lpr530al.pdf en http://www.sparkfun.com/datasheets/Sensors/IMU/LY530ALH.pdf kijkt) zult u zien dat de gevoeligheid 3.33mV/deg/s voor de 4xOUT is. Het is vrij gemakkelijk te vertalen in quids: gevoeligheid / 3.3 * 1023.
Dus in dit voorbeeld krijg ik:
0.00333/3.3*1023=1.0323.
NB: voor het vertalen van mV tot en met V eenvoudige gewoon delen door duizend.
De laatste vergelijking zal er zo uitzien:
gyroRate = (gyroAdc-gryoZero) / 1.0323
Het resultaat zal komen als graden per seconde. Om dit te vertalen in graden moet u weten de exacte tijd sinds de laatste lus. Gelukkig, de Arduino kreeg een eenvoudige opdracht om dit te doen: millis(). Met behulp van dat, kan men berekenen van het tijdsverschil (delta time) en daardoor het berekenen van de hoek van de gyro. De laatste vergelijking zal er zo uitzien:
gyroAngle += gyroRate * rekenwijze/1000
Helaas, drijft de gyro na verloop van tijd. Dat betekent dat het kan niet worden vertrouwd voor een langere tijdspanne, maar het is zeer nauwkeurig voor een korte tijd. Dit is wanneer de versnellingsmeter handig komt. Het hoeft niet elke drift, maar het is te instabiel voor kortere tijdspanne. Zal ik beschrijven hoe te combineren deze metingen in een tijdje, maar eerst zal ik beschrijven hoe te vertalen van de lezingen van de versnellingsmeter in iets nuttigs.
De versnellingsmeter
De versnellingsmeter meet de versnelling (g's) in drie dimensies. Om te vertalen van de analoge lezingen in graden, die u enkel hoeft te lezen van de as en de nul offset aftrekken als volgt:
accVal = accAdc-accZero
Waar accAdc is de analoge lezing en accZero is de waarde als er 0g staat-dit wordt berekend in het begin van de code, kijk in de sectie "Installatie". De waarde nul kan ook worden gevonden in het gegevensblad: http://www.sparkfun.com/datasheets/Components/SMD/adxl335.pdf. U zult zien dat de nulspanning op 0g is ongeveer 1, 5V, om te vertalen dit naar quids, je moet opnieuw gebruik maken van deze vergelijking: zeroVoltage / 3.3 * 1023.
Dus in dit voorbeeld krijg ik:
1.5/3.3*1023=465.
U kunt vervolgens berekent de toonhoogte en rollen met behulp van de volgende vergelijkingen:
toonhoogte = atan2 (accYval, accZval) + PI
Roll = atan2 (accXval, accZval) + PI
ATAN2 heeft een output bereik van - pi tot π (Zie http://en.wikipedia.org/wiki/Atan2), ik voeg π, zodat het bereik deze geconverteerd naar 0 en 2π.
Om deze te converteren van radialen naar graden we gewoon vermenigvuldig het resultaat met 57.295779513082320876798154814105 - dit vooraf is gedefinieerd in de Arduino IDE als RAD_TO_DEG.
Kalman-filter
Zoals ik al eerder heb uitgelegd de gyro is zeer nauwkeurig, maar de neiging om te drijven. De versnellingsmeter is een beetje instabiel, maar doet niet drijven. U kunt de precieze hoek berekenen met behulp van iets genaamd een Kalman-filter. Een gedetailleerde gids over hoe het wordt uitgevoerd kan gevonden worden op mijn blog: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/.
Als u wilt om iets een beetje meer eenvoudig te gebruiken, kunt u wat heet een aanvullende Filter. Het is vrij gemakkelijk te begrijpen en de wiskunde is veel eenvoudiger, omdat het werkt slechts in één stap.
De vergelijking kan er bijvoorbeeld als volgt uitzien:
hoek = 0.98 *(angle+gyro*dt) + 0,02 * acc - u kunt fine-tunen de nummers aan wat je wilt. Vergeet niet dat de som 1 moet zijn.
Voor mij was het resultaat uit de complementaire Filter zeer close (of bijna dezelfde) als degene berekend door de Kalman-filter.
Je hebt nu geleerd (hopelijk) het analoge gegevens ophalen uit IMU en vertalen naar iets nuttigs. Ik heb mijn eigen code bijgevoegd voor mijn 6DOF IMU (http://www.sparkfun.com/products/10010), maar met enkele wijziging iets, ik ben vrij zeker dat het mogelijk is te gebruiken met elke analoge gyro/versnellingsmeter.
Als je nog vragen hebt, viel vrij om een reactie hieronder.
Bronnen:
http://www.Arduino.CC/cgi-bin/yabb2/YaBB.pl?num=1284738418
http://www.x-Firm.com/?page_id=148
http://Web.mit.edu/First/Segway/
Update
Ik heb net een Processing-code die gegevens uit de Arduino op een mooie grafiek wordt afgedrukt. Zoals u in de video hieronder zien kunt is het filteren effectief stoppen. De licht blauwe lijn is de versnellingsmeter, de paarse lijn is de gyro, de zwarte lijn is de hoek berekend door het complementaire Filter en de rode lijn is de hoek berekend door de Kalman-filter. Zoals u de Kalman-filter is gewoon een beetje meer nauwkeurige zien kan (ik weet dat het moeilijk is te zien in de video) dan de aanvullende Filter, vooral wanneer ik schud het.
Ik heb mijn code, zowel de geactualiseerde code voor de Arduino en de verwerking code bijgevoegd. Het is ook mogelijk om de gegevens van de y-as te bekijken. Gewoon uncomment drawAxisY(); in de code.
Nieuwste firmware
Ik besloot om alle broncode op github, want het is veel makkelijker te onderhouden.
De nieuwste code kan nu worden gevonden op github: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter
Mijn Balancing robot
Hieronder is een video van mijn balancing robot. Het gebruikt dezelfde IMU en algoritme zoals beschreven in de post hierboven.
Kickstarter
Ik heb zojuist mijn balancing robot op Kickstarter: http://www.kickstarter.com/projects/tkjelectronics/balanduino-balancing-robot-kit
Kunt u overwegen het project steunen.
