Guide till gyro och accelerometer med Arduino inklusive Kalman filtrering
Jag återuppfört bara den, så ännu mer kommer att få användning av den. Koden kan hittas här:
https://github.com/TKJElectronics/example-sketch-for-IMU-including-Kalman-filter
Hallo alla
Jag köpte nyligen denna analoga 6DOF (sex frihetsgrader) IMU styrelse (http://www.sparkfun.com/products/10010) från watterott.com. Det använder tre gyron och tre accelerometrar för att beräkna vinklar i tre dimensioner.
Jag tittade en stund för lite kod online och hur du ansluter med dem. Jag lyckades efter många timmar av forskning att göra af exakt mätning av vinklar i två riktningar. Jag bestämde mig att skriva en kort guide för andra elektroniska entusiaster.
Det huvudsakliga syftet med denna guide är att lära andra att få några användbara data från deras IMU eller bara en gyro eller accelerometer. Koden för Arduino kan hittas på github: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter. Det borde vara ganska lätt att genomföra min kod till din egen sensor. Jag kommer inte beskriva alla detaljer om teorin bakom, istället kan du titta på källorna för mer information.
Innan du börjar måste du ansluta IMU enligt följande:
Acc_Gyro Arduino
3.3V <> — 3.3V
GND <> — GND
Gx4 X <> — AN0
Gx4 Y <> — AN1
Gx4 Z <> — AN2
ACC X <> — AN3
ACC Y <> — AN4
ACC Z <> — AN5
Också ansluta 3.3V till AREF stiftet på Arduino för mer precision.
Det är mycket viktigt att du inte ansluter sensorn till 5V - detta kommer att förstöra sensorn.
Nu du är redo för att läsa data från sensorn.
Att kommunicera med sensor är enkelt:
Gyrot mäter grader per sekund medan accelerometern mäter acceleration (Gustafssons) i tre dimensioner. Båda utgångarna mätningarna som en analog signal.
För att få detta översatt till grader måste du göra vissa koder:
Gyrot
Först måste du översätta quids (en siffra från 0-1023) till något användbart (detta är för en ADC med en 10 bitars upplösning, till exempel detta bör 4095 (2 ^ 12-1 = 4095) för 12 bitars ADC). Det gör använda jag bara den här enkel ekvation:
gyroRate = (gyroAdc-gyroZero) / känslighet - där gyroAdc är readed värdet från vår sensor, gyroZero är värdet när den är stillastående (detta görs i kod - titta i avsnittet "Installation") medan känslighet är känsligheten i databladet, men översatt till quids.
Om du tittar i de två gyron datablad (http://www.sparkfun.com/datasheets/Sensors/IMU/lpr530al.pdf och http://www.sparkfun.com/datasheets/Sensors/IMU/LY530ALH.pdf) kommer du se att känsligheten är 3.33mV/deg/s för 4xOUT. Att översätta dessa till quids är ganska enkelt: känslighet / 3.3 * 1023.
Så i detta exempel får jag:
0.00333/3.3*1023=1.0323.
Obs: för att översätta mV-V enkla delar bara med tusen.
Den sista ekvationen ut så här:
gyroRate = (gyroAdc-gryoZero) / 1.0323
Resultatet kommer att komma ut som grader per sekund. Att översätta detta till grader du måste veta den exakta tiden efter sista öglan. Lyckligtvis Arduino fick en enkel befalla till göra det: millis(). Genom att använda det, kan en beräkna skillnaden (delta tid) och därmed beräkna vinkeln på gyrot. Den sista ekvationen ut så här:
gyroAngle += gyroRate * dtime/1000
Tyvärr, gyrot drivor över tiden. Det innebär att det inte kan vara betrodda för en längre tid som gått, men det är mycket exakt för en kort tid. Detta är när accelerometern är praktiskt. Det har inte någon drift, men det är alltför instabil för kortare tid. Jag kommer att beskriva hur man kombinerar dessa mätningar på ett tag, men först kommer jag att beskriva hur man översätter avläsningarna från accelerometern till något användbart.
Accelerometern
Accelerometern mäter acceleration (Gustafssons) i tre dimensioner. För att översätta analog avläsningar i grader behöver du bara läsa axel och subtrahera noll offset som så:
accVal = accAdc-accZero
Där accAdc är den analoga avläsningen och accZero är värdet när den läser 0g - detta kan beräknas i början av koden, titta i avsnittet "Installation". Nollvärdet kan också hittas i databladet: http://www.sparkfun.com/datasheets/Components/SMD/adxl335.pdf. Du kommer att se att den noll spänningen på 0g är cirka 1.5V, att översätta detta till quids, du har igen att använda denna ekvation: zeroVoltage / 3.3 * 1023.
Så i detta exempel får jag:
1.5/3.3*1023=465.
Du kan då beräkna planen och rulla med hjälp av följande ekvationer:
pitch = ARCTAN2 (accYval, accZval) + PI
rulle = ARCTAN2 (accXval, accZval) + PI
ARCTAN2 har utgång intervallet - π till π (se http://en.wikipedia.org/wiki/Atan2), jag helt enkelt lägga till π, så spänna det konverteras till 0 till 2π.
Att konvertera den från radianer till grader vi helt enkelt multiplicera resultatet med 57.295779513082320876798154814105 - detta är fördefinierade i Arduino IDE som RAD_TO_DEG.
Kalmanfilter
Som jag förklarade tidigare gyrot är mycket exakt, men tenderar att glida. Accelerometern är lite instabil, men glida inte. Du kan beräkna den exakta vinkeln med hjälp av något som kallas en Kalmanfilter. En detaljerad guide om hur det genomförs finns på min blogg: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/.
Om du vill använda något lite mer enkelt, kan du använda vad som kallas ett kompletterande Filter. Det är ganska lätt att förstå och matten är mycket enklare, eftersom det fungerar bara i ett enda steg.
Ekvationen kan till exempel se ut så här:
vinkel = 0,98 *(angle+gyro*dt) + 0,02 * acc - du kan finjustera numrerar till vad du vill. Kom bara ihåg att summan måste vara 1.
För mig var resultatet från kompletterande filtret mycket nära (eller nästan samma) som som beräknats av Kalman-filter.
Du har nu lärt sig (förhoppningsvis) att få analog data från IMU och översätta det till något nyttigt. Jag har fäst min egen kod för min 6DOF IMU (http://www.sparkfun.com/products/10010), men med några lite ändringar, jag är ganska säker på att det är möjligt att använda den med någon analog gyro/accelerometer.
Om du har någon fråga, föll fritt att skriva en kommentar nedan.
Källor:
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/
Uppdatering
Jag har precis avslutat en behandling kod som skriver ut data från Arduino på en fin graf. Som ni kan se i videon nedan är filtreringen avsluta effektivt. Ljus blå linjen är accelerometern, den lila linjen är gyrot, den svarta linjen är vinkeln beräknas av kompletterande filtret och den röda linjen är vinkeln beräknas av Kalmanfilter. Som ni kanske ser Kalmanfilter är bara lite mer exakt (jag vet att det är svårt att se i videon) än den kompletterande Filter, särskilt när jag skakar den.
Jag har bifogat min kod, både uppdaterad koden för Arduino och bearbetning koden. Det är också möjligt att se data från y-axeln. Kommentera bara bort drawAxisY(); i koden.
Nyaste firmware
Jag beslutat att lägga all källkod på github, eftersom det är mycket lättare att underhålla.
Nyaste koden finns nu på github: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter
Min Balancing robot
Nedan är en video av min balanserande robot. Den använder samma IMU och algoritmen som beskrivs i inlägget ovan.
Kickstarter
Jag har just släppt min balanserande robot på kicken: http://www.kickstarter.com/projects/tkjelectronics/balanduino-balancing-robot-kit
Överväg att stödja projektet.