Accelerometer & Gyro handledning (3 / 3 steg)

Steg 3: Kombinera Accelerometer och Gyro

Att sätta allt ihop - kombinera accelerometer och gyroskop data.

Om du läser denna artikel du förmodligen förvärvade eller planerar att skaffa en IMU enhet, eller förmodligen du planerar att bygga en från separata accelerometer och gyroskop enheter.

Det första steget i att använda en kombination IMU enhet som kombinerar en accelerometer och ett gyroskop är att anpassa deras koordinatsystem. Det enklaste sättet att göra det är att välja det koordinerade systemet av accelerometer som din hänvisning koordinatsystem. De flesta accelerometer datablad visar den riktning av X, Y, Z-axlarna i förhållande till bilden av den fysiska chip eller enhet. Här är till exempel den riktningar av X, Y, Z-axlar som visas i specifikationerna för Acc_Gyro styrelsen:

Nästa steg är:

• Identifiera gyroskop utgångarna som motsvarar RateAxz, RateAyz värden som diskuterats ovan. Avgöra om dessa utgångar måste inverteras på grund av fysiska position av gyroskop i förhållande till accelerometern

Förutsätt inte att om ett gyroskop har en utgång märkt X eller Y, det motsvarar någon axel i koordinatsystemet accelerometer, även om denna utgång är del av en IMU enhet. Det bästa sättet är att testa den. Förutsatt att du har fixat positionen för gyroskop i förhållande till accelerometern. Det antas att gyro och accelerometer gränsar är parallella med varandra, dvs du placerar gyrot vinkel multipel av 90deg i förhållande till den accelerometer flisa. Om du förvärvat en IMU ombord chanserna är att de är redan justerad hitåt. Vi kommer inte att diskutera i denna artikel modeller där gyroskop är placerad i en oregelbunden vinkel i förhållande till accelerometer (Låt oss säga 45 eller 30 grader), även om detta kan vara användbart i vissa program.

Här är en prov sekvens bestämma vilka produktionen av gyroskopet motsvarar RateAxz värdet diskuterats ovan.

-starta från att placera enheten i horisontell position. Både X och Y utgångar av accelerometer skulle utspänning den viktlösa (till exempel för Acc_Gyro ombord på detta är 1.65V)

-nästa börja rotera enheten runt Y-axeln, ett annat sätt att säga det är att du roterar enheten i XZ-planet, så att X och Z accelerometer utgångar förändring och Y produktionen förblir konstant.

-samtidigt vrida enheten vid en konstant hastighet notera vilka gyroskop utgång förändringar, andra gyroskop utgångarna bör vara konstant

-gyroskop utdata som ändrats under rotation kring Y-axeln (rotation i XZ-planet) ger indatavärdet för AdcGyroXZ, från vilken vi beräkna RateAxz

-det sista steget är att säkerställa rotationsriktningen motsvarar vår modell, i vissa fall kan du behöva Invertera RateAxz värdet på grund av fysiska position av gyroskop i förhållande till accelerometern

-utför igen ovanstående provning, rotera enheten runt Y-axeln, den här gången övervaka X produktionen av accelerometer (AdcRx i vår modell). Om AdcRx växer (de första 90 graderna av rotation från horisontellt läge), bör då AdcGyroXZ också växa. Annars måste du invertera RateAxz, du kan uppnå detta genom att införa en faktor för Eq.3, tecken enligt följande:

RateAxz = InvertAxz * (AdcGyroXZ * Vref / 1023 - VzeroRate) / känslighet, där InvertAxz är 1 eller -1

samma test sockerrör vara gjort för RateAyz, genom att rotera enheten runt X-axeln, och du kan identifiera vilka gyroskop utgång motsvarar RateAyz och om den behöver inverteras. När du har värdet för InvertAyz, bör du använda följande formel för att beräkna RateAyz:

RateAyz = InvertAyz * (AdcGyroYZ * Vref / 1023 - VzeroRate) / känslighet

Om du skulle göra dessa tester på Acc_Gyro ombord skulle du få följande resultat:

-utdata PIN-koden för RateAxz är GX4 och InvertAxz = -1.
-utdata PIN-koden för RateAyz är GY4 och InvertAyz = -1

Från och med nu kommer vi anser att du har setup din IMU så att du kan beräkna korrekta värden för Axr, Ayr, Azr (som definierade Del1. Accelerometer) och RateAxz, RateAyz (enligt definitionen i del 2. Gyroskop). Nästa ska vi analysera relationerna mellan dessa värden att visa sig användbart att få mer exakt uppskattning av lutningen på enheten i förhållande till markplanet.

Du kanske frågar dig själv genom denna punkt, om accelerometer modell redan gav oss lutning vinklar av Axr, Ayr, Azr varför skulle vi vilja att bry sig med gyroskop data? Svaret är enkelt: accelerometer data kan inte alltid vara betrodda 100%. Det finns flera skäl, minns accelerometer tröghetsbaserad kraft, kan sådan styrka kan orsakas av gravitation (och helst endast vid gravitation), men det kan också orsakas av acceleration (rörelse) av enheten. Som ett resultat även om accelerometer är en relativt stabil, är det fortfarande mycket känslig för vibrationer och mekaniskt buller i allmänhet. Detta är den främsta anledningen varför de flesta IMU-system använder ett gyroskop för att jämna ut eventuella accelerometer fel. Men hur sker detta? Och gyroskopet fri från buller?

Gyroskopet är inte fritt från buller eftersom den mäter rotation det emellertid mindre känsliga för linjära mekaniska rörelser, typ av buller som accelerometer som lider, men gyroskop har andra typer av problem som till exempel drift (inte kommer tillbaka till nollskattesatsen värde när rotation stannar). Som genomsnittet av data som kommer från accelerometer och gyroskop kan vi ändå få en relativt bättre uppskattning av nuvarande enhet böjelse än vi skulle få med hjälp av accelerometer data ensam.

I nästa steg kommer jag att införa en algoritm som var inspirerad av några idéer används i Kalmanfilter, det emellertid vid långt mer enkel och lättare att genomföra om inbäddade enheter. Innan dess ska vi se först vad vi vill ha vår algoritm för att beräkna. Jo, det är riktningen av gravitation force vector R = [Rx, Ry, Rz] från vilket vi kan härleda andra värden som Axr, Ayr, Azr eller cosX, mysiga, cosZ som ger oss en uppfattning om lutningen på vår enhet i förhållande till jordplanet, vi diskutera förhållandet mellan dessa värden i Del1. Man kan säga - vi inte redan har dessa värden Rx, Ry, Rz från Eq.2 i Del1? Ja Ja, men kom ihåg att dessa värden härrör från accelerometer data, så om du skulle vara att använda dem direkt i din ansökan kan du få mer buller än din ansökan kan tolerera. Låt oss undvika ytterligare förvirring omdefiniera accelerometer mätningarna enligt följande:

Racc - är trög kraft vektorn mätt med accelerometer, som består av följande komponenter (projektioner på X, Y, Z-axlar):

RxAcc = (AdcRx * Vref / 1023 - VzeroG) / känslighet
RyAcc = (AdcRy * Vref / 1023 - VzeroG) / känslighet
RzAcc = (AdcRz * Vref / 1023 - VzeroG) / känslighet

Hittills har vi en uppsättning av uppmätta värden som vi kan få rent från accelerometer ADC värden. Vi kallar denna uppsättning data en "vector" och vi kommer att använda följande notationen.

Racc = [RxAcc, RyAcc, RzAcc]

Eftersom dessa komponenter för Racc kan erhållas från accelerometer data, kan vi överväga det indata till vår algoritm.

Observera att eftersom Racc mäter den gravitation kraft blir du rätt om du antar att längden av denna vektor enligt definitionerna är lika eller nära 1 g.

| Racc| = SQRT(RxAcc^2 +RyAcc^2 + RzAcc^2),

Emellertid vara säker på att det är vettigt att uppdatera denna vektor enligt följande:

Racc(Normalized) = [RxAcc / | Racc| , RyAcc / | Racc| , RzAcc / | Racc|].

Detta kommer att säkerställa din normaliserade Racc vektor är alltid 1.

Därefter kommer vi att introducera en ny vektor och vi kallar det

Resten = [RxEst, RyEst, RzEst]

Detta kommer att bli resultatet av vår algoritm, dessa korrigerade värden baserat på gyroskop data och utifrån tidigare beräknade data.

Här är vad vår algoritm gör:
-accelerometer berättar: "Du är nu på plats Racc"
-Vi säger "Tack, men låt mig kontrollera",
-sedan rätta till informationen med gyroskop data samt med senaste resten data och vi ut en ny uppskattad vektor resten.
-Vi anser resten vara vår "bästa insats" om den aktuella positionen för enheten.

Låt oss se hur vi kan få det att fungera.

Vi börjar vår sekvens genom att lita på våra accelerometer och tilldela:

Rest(0) = Racc(0)

Förresten kom ihåg resten och Racc är vektorer, så ovanstående ekvation är bara ett enkelt sätt att skriva 3 uppsättningar av likställande, och undvika upprepning:

RxEst(0) = RxAcc(0)
RyEst(0) = RyAcc(0)
RzEst(0) = RzAcc(0)

Nästa vi gör regelbundna mätningar med samma tidsintervall T sekunder, och vi kommer få nya mätningar som vi ska definiera som Racc(1), Racc(2), Racc(3) och så vidare. Vi ska också utfärda nya beräkningar på varje tidsintervall Rest(1), Rest(2), Rest(3) och så vidare.

Anta att vi är på steg n. Vi har två kända uppsättningar med värden som vi vill använda:

Rest(n-1) - våra tidigare uppskattning, med Rest(0) = Racc(0)
Racc(n) - vår nuvarande accelerometer mätning

Innan vi kan beräkna Rest(n), låt oss införa ett nytt mätvärde, som vi kan få från våra gyroskop och en tidigare uppskattning.

Vi kallar det Rgyro och det är också en vektor som består av 3 delar:

Rgyro = [RxGyro, RyGyro, RzGyro]

Vi ska beräkna denna vektor en komponent på en gång. Vi börjar med RxGyro.

Låt oss börja genom att observera följande relationen i vår gyroskop modell, från den rätvinkliga triangeln bildas av Rz och Rxz vi kan härleda som:

Tan(Axz) = Rx/Rz = > Axz = atan2(Rx,Rz)

ARCTAN2 kan vara en funktion som du aldrig använt förut, det liknar atan, förutom det returnerar värden i rad (-PI, PI) i motsats till (-PI/2, PI/2) som returneras av atan, och det tar 2 argument i stället för en. Det ger oss möjlighet att konvertera de två värdena av Rx, Rz till vinklar i hela skalan av 360 grader (-PI att PI). Du kan läsa mer om ARCTAN2 här.

Så att veta RxEst(n-1), och RzEst(n-1) vi kan hitta:

Axz(n-1) = ARCTAN2 (RxEst(n-1), RzEst(n-1)).

Kom ihåg att gyroskop mäter graden av förändring av Axz vinkeln. Så kan vi beräkna den nya vinkeln Axz(n) enligt följande:

Axz(n) = Axz(n-1) + RateAxz(n) * T

Kom ihåg att RateAxz kan erhållas från våra gyroskop ADC avläsningar. En mer exakt formel kan använda en genomsnittlig rotation ränta beräknas på följande sätt:

RateAxzAvg = (RateAxz(n) + RateAxz(n-1)) / 2
Axz(n) = Axz(n-1) + RateAxzAvg * T

På samma sätt som vi kan hitta:

Ayz(n) = Ayz(n-1) + RateAyz(n) * T

OK så nu har vi Axz(n) och Ayz(n). Där ska vi gå härifrån dra av RxGyro/RyGyro? Från Eq. 1 kan vi skriva vektor Rgyro längd enligt följande:

| Rgyro| = SQRT(RxGyro^2 + RyGyro^2 + RzGyro^2)

Också eftersom vi normaliserade våra Racc vektor, kan vi anta att det är längd är 1 och inte har förändrats efter rotationen, så det är relativt säkert att skriva:

| Rgyro| = 1

Låt oss anta ett tillfälligt kortare notation för beräkningarna nedan:

x = RxGyro, y = RyGyro, z = RzGyro

Nyttjande av förbindelser över vi kan skriva:

x = x / 1 = x / SQRT(x^2+y^2+z^2)

Låt oss dela täljaren och nämnaren av fraktionen av SQRT (x ^ 2 + z ^ 2)

x = (x / SQRT (x ^ 2 + z ^ 2)) / SQRT ((x ^ 2 + y ^ 2 + z ^ 2) / (x ^ 2 + z ^ 2))

Observera att x / SQRT (x ^ 2 + z ^ 2) = sin(Axz), så:

x = sin(Axz) / SQRT (1 + y ^ 2 / (x ^ 2 + z ^ 2))

Nu multiplicera täljare och nämnare i bråk inne SQRT av z ^ 2

x = sin(Axz) / SQRT (1 + y ^ 2 * z ^ 2 / (z ^ 2 * (x ^ 2 + z ^ 2)))

Observera att z / SQRT (x ^ 2 + z ^ 2) = cos(Axz) och y / z = tan(Ayz), så till slut:

x = sin(Axz) / SQRT (1 + cos (Axz) ^ 2 * tan (Ayz) ^ 2)

Gå tillbaka till våra notation får vi:

RxGyro = sin(Axz(n)) / SQRT (1 + cos(Axz(n)) ^ 2 * tan(Ayz(n)) ^ 2)

samma sätt finner vi att

RyGyro = sin(Ayz(n)) / SQRT (1 + cos(Ayz(n)) ^ 2 * tan(Axz(n)) ^ 2)

Nu, äntligen kan vi hitta:

RzGyro = tecken (RzGyro) * SQRT (1 - RxGyro ^ 2 - RyGyro ^ 2).

Där Sign(RzGyro) = 1 när RzGyro > = 0 och Sign(RzGyro) =-1 när RzGyro < 0.

Ett enkelt sätt att uppskatta detta är att ta:

Sign(RzGyro) = Sign(RzEst(n-1))

I praktiken var försiktig när RzEst(n-1) är nära 0. Du kan hoppa över den gyro fasen helt och hållet i detta fall och tilldelar: Rgyro = Rest(n-1). RZ används som en referens för att beräkna Axz och Ayz vinklar och när det är nära 0, värden kan oveflow och utlösa dåliga resultat. Du ska i domänen av stora flytande punkt nummer var tan() / atan() funktion implementeringar kan sakna precision.

Så låt oss sammanfatta vad vi har hittills, vi är på steg n av vår algoritm och vi har beräknat följande värden:

Racc - nuvarande avläsningar från våra accelerometer
Rgyro - från Rest(n-1) och nuvarande gyroskop avläsningar

Vilka värden använder vi för att beräkna den uppdaterade uppskattningen Rest(n)? Du gissade förmodligen att vi ska använda båda. Vi använder ett viktat medelvärde, så att:

Rest(n) = (Racc * w1 + Rgyro * w2) / (w1 + w2)

Vi kan förenkla denna formel genom att dividera täljare och nämnare i bråket med w1.

Rest(n) = (Racc * w1/w1 + Rgyro * w2/w1) / (w1/w1 + w2/w1)

och efter ersätta w2/w1 = wGyro får vi:

Rest(n) = (Racc + Rgyro * wGyro) / (1 + wGyro)

I den ovan forumula berättar wGyro hur mycket vi litar på våra gyro jämfört med våra accelerometer. Detta värde kan väljas experimentellt vanligtvis värden mellan 5..20 kommer att leda till goda resultat.

Den största skillnaden för den här algoritmen från Kalmanfilter är att denna vikt är relativt fast, medan i Kalman filtret uppdateras permanent baserad på uppmätt buller av accelerometer avläsningar. Kalmanfilter är inriktad på att ge dig "det bästa" teoretiska resultat, medan denna algoritm kan ge dig resultat "tillräckligt bra" för din praktiska tillämpning. Du kan implementera en algoritm som justerar wGyro beroende på några buller faktorer som man mäter, men fasta värden kommer att fungera bra för de flesta applikationer.

Vi är ett steg från att få våra uppdaterade beräknade värden:

RxEst(n) = (RxAcc + RxGyro * wGyro) / (1 + wGyro)
RyEst(n) = (RyAcc + RyGyro * wGyro) / (1 + wGyro)
RzEst(n) = (RzAcc + RzGyro * wGyro) / (1 + wGyro)

Nu låt oss normalisera vektorn igen:

R = SQRT(RxEst(n) ^ 2 + RyEst (n) ^ 2 + RzEst (n) ^ 2)

RxEst(n) = RxEst (n) / r
RyEst(n) = RyEst (n) / r
RzEst(n) = RzEst (n) / r

Och vi är redo att upprepa vår slinga igen.

Denna guide ursprungligen visades på starlino.com, jag har gjort några ljusa redigeringar och åter skrivit det med tillstånd. Tack Starlino!