Stap 3: Het combineren van de versnellingsmeter en de Gyro
Zet het allemaal samen - versnellingsmeter en gyroscoop gegevens combineren.
Als je dit artikel leest u waarschijnlijk verworven of van plan bent om het verwerven van een IMU apparaat, of waarschijnlijk u van plan bent om een van de afzonderlijke versnellingsmeter en gyroscoop apparaten te bouwen.
De eerste stap bij het gebruik van een combinatie IMU apparaat dat een versnellingsmeter en een gyroscoop combineert is om uit te lijnen hun coördinatenstelsels. De eenvoudigste manier om dit te doen is te kiezen van het coördinatensysteem van de versnellingsmeter als uw referentie-coördinatenstelsel. Meeste versnellingsmeter gegevensbladen wordt de richting van X, Y, Z-as ten opzichte van het imago van de fysieke chip of het apparaat weergegeven. Bijvoorbeeld zijn hier de richtingen van X, Y, Z-as zoals in de specificaties voor het Acc_Gyro -board:
Volgende stappen zijn:
- Het identificeren van de gyroscoop uitgangen die overeenkomen met RateAxz, RateAyz waarden hierboven besproken. Bepaal of deze uitgangen moeten worden omgekeerd als gevolg van fysieke positie van de gyroscoop ten opzichte van de versnellingsmeter
Ga er niet van dat, als een gyroscoop heeft een output gemarkeerd X of Y, het zal overeenkomen met elke willekeurige as in het coördinatensysteem van de versnellingsmeter, zelfs als deze uitgang deel van een eenheid voor IMU is. De beste manier is om het te testen. Ervan uitgaande dat u de positie van de gyroscoop ten opzichte van de versnellingsmeter hebben vastgesteld. Er wordt van uitgegaan dat de gyro en versnellingsmeter grenzen zijn parallel aan elkaar, d.w.z. u plaatst de gyro onder een hoek veelvoud van 90deg ten opzichte van de versnellingsmeterspaander. Indien u een IMU board kansen zijn verkregen dat ze al op deze manier zijn uitgelijnd. We not gonna bespreken in dit artikel modellen waar de gyroscoop in een onregelmatige hoek ten opzichte van de versnellingsmeter wordt geplaatst (laten we zeggen 45 of 30 graden), hoewel dit nuttig zijn in sommige toepassingen wellicht.
Hier is een opeenvolging van de steekproef om te bepalen welke uitgang van de gyroscoop komt overeen met RateAxz waarde hierboven besproken.
-starten vanaf het apparaat in horizontale positie te plaatsen. Zowel X- en Y-uitgangen van de versnellingsmeter zou uitgangsspanning de zero-g (voor voorbeeld voor Acc_Gyro aan boord van dit 1.65V)
-de volgende keer start het apparaat rond de Y-as draaien, een andere manier om het te zeggen is dat u het apparaat in XZ-vlak, zodat de X en Z versnellingsmeter uitgangen wijzigen en Y-uitgang blijft constant draaien.
-terwijl het draaien van het apparaat op een opmerking van de constante snelheid welke gyroscoop uitgang wijzigingen, de andere uitgangen van de gyroscoop constant moeten blijven
-de gyroscoop uitvoer die tijdens de draaihoek om de Y-as (rotatie in XZ-vlak) gewijzigd zorgt de invoerwaarde voor AdcGyroXZ, waaruit wij RateAxz berekenen
-de laatste stap is om ervoor te zorgen de draairichting correspondeert met ons model, in sommige gevallen wellicht moet het omkeren van de waarde van de RateAxz als gevolg van fysieke positie van de gyroscoop ten opzichte van de versnellingsmeter
-opnieuw uitvoeren van de bovenstaande test, het apparaat rond de Y-as draaien, ditmaal controleren de output van de X van de versnellingsmeter (AdcRx in ons model). Als AdcRx groeit (de eerste 90 graden voor rotatie van horizontale positie), moet dan AdcGyroXZ ook groeien. Anders moet u omkeren RateAxz, u kunt dit bereiken door de invoering van een teken factor in Eq.3, als volgt:
RateAxz = InvertAxz * (AdcGyroXZ * Vref / 1023 - VzeroRate) / gevoeligheid, waar InvertAxz 1 of -1 is
dezelfde test suikerriet worden gedaan voor RateAyz, door het draaien van het apparaat rond de X-as, en kunt u identificeren welke gyroscoop uitgang komt overeen met RateAyz, en als het moet worden omgekeerd. Zodra u de waarde voor InvertAyz hebt, moet u de volgende formule voor het berekenen van RateAyz:
RateAyz = InvertAyz * (AdcGyroYZ * Vref / 1023 - VzeroRate) / gevoeligheid
Als u deze tests aan boord van de Acc_Gyro zou doen u zou krijgen op de volgende resultaten:
-de pin van de output voor RateAxz is GX4 en InvertAxz = -1.
-de pin van de output voor RateAyz is GY4 en InvertAyz = -1
Vanaf dit punt overwegen we dat u ingesteld uw IMU op zodanige wijze hebt dat u juiste waarden voor Axr, Ayr, Azr (zoals gedefinieerde deel 1 berekenen kunt. Versnellingsmeter) en RateAxz, RateAyz (zoals gedefinieerd in deel 2. Gyroscoop). Vervolgens zullen we de betrekkingen tussen deze waarden die nuttig zijn blijken bij het verkrijgen van meer accurate schatting van de hellingshoek van het apparaat ten opzichte van de massaplaat analyseren.
U zou kunnen vragen jezelf bij dit punt, als versnellingsmeter model al gaf ons neiging hoeken van Axr, Ayr, Azr, waarom zouden we willen met de gegevens van de gyroscoop de moeite? Het antwoord is simpel: versnellingsmeter-gegevens niet altijd kunnen worden vertrouwd 100%. Er zijn verschillende redenen, vergeet niet dat versnellingsmeter maatregelen inertial kracht, dergelijke macht kan worden veroorzaakt door de zwaartekracht (en idealiter alleen door zwaartekracht), maar het kan ook worden veroorzaakt door versnelling (verkeer) van het apparaat. Dientengevolge zelfs als versnellingsmeter in een relatief stabiele staat, is het nog steeds zeer gevoelig voor trillingen en mechanische lawaai in het algemeen. Dit is de belangrijkste reden waarom meeste IMU systemen een gyroscoop gebruiken voor het gladstrijken van eventuele fouten van de versnellingsmeter. Maar hoe is dit gedaan? En is de gyroscoop vrij van ruis?
De gyroscoop is niet vrij van ruis maar omdat het rotatie meet minder is gevoelig voor lineaire mechanische bewegingen, het soort lawaai dat versnellingsmeter lijdt, maar gyrokompassen hebben andere soorten problemen zoals bijvoorbeeld drift (niet terugkomen nader waarde wanneer rotatie stopt). Toch kunnen we een relatief beter inschatten van de huidige apparaat helling dan wij verkrijgen zouden door het gebruik van de versnellingsmeter-gegevens alleen door het gemiddelde van de gegevens die afkomstig zijn van de versnellingsmeter en gyroscoop te kunnen verkrijgen.
In de volgende stappen die ik zal voorstellen een algoritme dat werd geïnspireerd door enkele ideeën in Kalman-filter gebruikt, is het echter veel eenvoudiger en gemakkelijker te implementeren op ingesloten apparaten. Daarvoor laten we eerst zien wat we willen ons algoritme voor het berekenen van. Nou, het is de richting van de zwaartekracht kracht vector R = [Rx, Ry, Rz] die we kunnen ontlenen andere waarden als Axr, Ayr, Azr of cosX, gezellige, cosZ, dat ons een idee over de helling van onze apparaat ten opzichte van het grondvlak geeft, we bespreken de relatie tussen deze waarden in deel 1. Men zou kunnen zeggen - niet we hebben al deze waarden Rx, Ry, Rz van Eq.2 in deel 1? Nou ja, maar vergeet niet dat deze waarden zijn afgeleid van versnellingsmeter-gegevens alleen, dus als je zou ze rechtstreeks in uw toepassing te gebruiken kan je meer ruis dan uw toepassing kan tolereren. Om te voorkomen dat verdere verwarring laten we opnieuw de versnellingsmeter metingen te definiëren als volgt:
Racc - is de vector van de traagheids kracht zoals gemeten door de versnellingsmeter, die uit de volgende onderdelen bestaat (projecties op de X, Y, Z-assen):
RxAcc = (AdcRx * Vref / 1023 - VzeroG) / gevoeligheid
RyAcc = (AdcRy * Vref / 1023 - VzeroG) / gevoeligheid
RzAcc = (AdcRz * Vref / 1023 - VzeroG) / gevoeligheid
Tot nu toe hebben we een aantal gemeten waarden die wij puur vanaf versnellingsmeter ADC waarden kunnen downloaden. We noemen deze set van gegevens een "vector" en we zullen de volgende notatie gebruiken.
Racc = [RxAcc, RyAcc, RzAcc]
Omdat deze onderdelen voor Racc kunnen versnellingsmeter-gegevens worden verkregen, kunnen we het een bijdrage aan ons algoritme overwegen.
Houd er rekening mee dat omdat Racc de kracht van de zwaartekracht zult u correct meet als u aannemen dat de lengte van deze vector als volgt gedefinieerd gelijk is of bijna 1 g.
| Racc| = SQRT(RxAcc^2 +RyAcc^2 + RzAcc^2),
Maar om zeker te zijn is het zinvol om deze vector als volgt bijwerken:
Racc(Normalized) = [RxAcc / | Racc| , RyAcc / | Racc| , RzAcc / | Racc|].
Hierdoor zal dat de lengte van uw genormaliseerde Racc vector is altijd 1.
Vervolgens zal introduceren wij een nieuwe vector en we noemen het
Rest = [RxEst, RyEst, RzEst]
Dit zal de output van ons algoritme, dit zijn de gecorrigeerde waarden op basis van gegevens van de gyroscoop en gebaseerd op afgelopen geschatte gegevens.
Hier is wat ons algoritme zal doen:
-versnellingsmeter vertelt ons: "Gij zijt nu op positie Racc"
-we zeggen "Dank u, maar laat me check",
-Corrigeer vervolgens deze informatie met gegevens van de gyroscoop en afgelopen Rest gegevens en we een nieuwe geschatte vector Rest output.
-Wij Rest als onze "beste weddenschap" over de huidige positie van het apparaat.
Laten we eens kijken hoe we kunnen werken.
We beginnen onze reeks door te vertrouwen onze versnellingsmeter en toewijzen:
Rest(0) = Racc(0)
Door de manier herinneren en Racc zijn vectoren, zodat de bovenstaande vergelijking is enkel een eenvoudige manier om te schrijven 3 sets van vergelijkingen, en voorkomen van herhaling:
RxEst(0) = RxAcc(0)
RyEst(0) = RyAcc(0)
RzEst(0) = RzAcc(0)
Volgende wij regelmatige metingen doen op gelijke tijdsintervallen van T seconden, en we zullen krijgen nieuwe metingen die we als Racc(1), Racc(2), Racc(3), enzovoort definiëren zullen. We zullen ook afgifte van nieuwe ramingen op elke tijdsintervallen Rest(1), Rest(2), Rest(3), enzovoort.
Stel dat we je op de stap van n. We hebben twee bekende sets met waarden die wij zou willen gebruiken:
Rest(n-1) - onze eerdere raming, met Rest(0) = Racc(0)
Racc(n) - onze Stroommeting versnellingsmeter
Voordat we Rest(n) berekenen kunnen, laten we even een nieuwe gemeten waarde, die we vanaf onze gyroscoop en een eerdere raming kunnen downloaden.
We noemen het Rgyro, en het is ook een vector bestaande uit 3 onderdelen:
Rgyro = [RxGyro, RyGyro, RzGyro]
We zullen dit één component van de vector berekenen op een moment. We beginnen met RxGyro.
Laten we beginnen door te kijken naar de volgende relatie in onze gyroscoop model, uit de rechte hoek driehoek gevormd door Rz en Rxz we kunnen afleiden dat:
Tan(Axz) = Rx/Rz = > Axz = atan2(Rx,Rz)
ATAN2 misschien wel een functie die u nooit gebruikt voor, het is vergelijkbaar met atan, behalve binnen het bereik van waarden geretourneerd (-PI, PI) in tegenstelling tot (-PI/2, PI/2) zoals die wordt geretourneerd door atan, en het neemt 2 argumenten in plaats van één. Het laat ons toe om de twee waarden van Rx, Rz draaien in het volledige bereik van 360 graden converteren (-PI tot PI). U kunt meer lezen over atan2 hier.
Zo weten RxEst(n-1) en RzEst(n-1) vinden we:
Axz(n-1) = atan2 (RxEst(n-1), RzEst(n-1)).
Vergeet niet dat gyroscoop meet de snelheid van verandering van de hoek van de Axz. Zo kunnen we schatten de nieuwe hoek Axz(n) als volgt:
Axz(n) = Axz(n-1) + RateAxz(n) * T
Vergeet niet dat RateAxz kan worden verkregen bij onze gyroscoop ADC lezingen. Een meer nauwkeurige formule kunt een percentage van de gemiddelde rotatie als volgt berekend:
RateAxzAvg = (RateAxz(n) + RateAxz(n-1)) / 2
Axz(n) = Axz(n-1) + RateAxzAvg * T
Dezelfde manier we vinden:
Ayz(n) = Ayz(n-1) + RateAyz(n) * T
OK dus nu hebben we Axz(n) en Ayz(n). Waar gaan we vanaf hier op aftrek van de RxGyro/RyGyro? Van Eq. 1 kunnen we de lengte van de vector Rgyro als volgt schrijven:
| Rgyro| = SQRT(RxGyro^2 + RyGyro^2 + RzGyro^2)
Ook omdat we onze Racc vector genormaliseerd, mogen we aannemen dat het lengte 1 is en het is niet veranderd na de rotatie, dus het is relatief veilig om te schrijven:
| Rgyro| = 1
Laten we vast een tijdelijke kortere notatie voor de berekeningen hieronder:
x = RxGyro, y = RyGyro, z = RzGyro
Met de betrekkingen boven we kunt schrijven:
x = x / 1 = x / SQRT(x^2+y^2+z^2)
Laten we het verdelen van teller en noemer van de breuk door SQRT (x ^ 2 + z ^ 2)
x = (x / SQRT (x ^ 2 + z ^ 2)) / SQRT ((x ^ 2 + y ^ 2 + z ^ 2) / (x ^ 2 + z ^ 2))
Merk op dat x / SQRT (x ^ 2 + z ^ 2) = sin(Axz), dus:
x = sin(Axz) / SQRT (1 + y ^ 2 / (x ^ 2 + z ^ 2))
Nu vermenigvuldig de teller en noemer van de breuk binnen SQRT door z ^ 2
x = sin(Axz) / SQRT (1 + y ^ 2 * z ^ 2 / (z ^ 2 * (x ^ 2 + z ^ 2)))
Merk op dat z / SQRT (x ^ 2 + z ^ 2) = cos(Axz) en y / z = tan(Ayz), dus uiteindelijk:
x = sin(Axz) / SQRT (1 + cos (Axz) ^ 2 * tan (Ayz) ^ 2)
Terug te gaan naar onze notatie krijgen we:
RxGyro = sin(Axz(n)) / SQRT (1 + cos(Axz(n)) ^ 2 * tan(Ayz(n)) ^ 2)
dezelfde manier vinden we dat
RyGyro = sin(Ayz(n)) / SQRT (1 + cos(Ayz(n)) ^ 2 * tan(Axz(n)) ^ 2)
Nu, eindelijk kan vinden we:
RzGyro = teken (RzGyro) * SQRT (1 - RxGyro ^ 2 - RyGyro ^ 2).
Waar Sign(RzGyro) = 1 als RzGyro > = 0, en Sign(RzGyro)-1 wanneer = RzGyro < 0.
Een eenvoudige manier om te schatten dit is om te nemen:
Sign(RzGyro) = Sign(RzEst(n-1))
In de praktijk wees voorzichtig wanneer RzEst(n-1) dicht bij 0 is. U kunt overslaan de gyro fase helemaal in dit geval en toewijzen: Rgyro = Rest(n-1). RZ wordt gebruikt als referentie voor de berekening van de Axz en de Ayz hoeken en wanneer het is dicht bij 0, waarden kunnen oveflow en trigger slechte resultaten. U zult in domein van grote drijvende punt nummers waar tan() / atan() functie implementaties kunnen gebrek aan precisie.
Dus laten we recapituleren wat we hebben tot nu toe, we zijn bij stap n van ons algoritme en we hebben de volgende waarden berekend:
Racc - huidige lezingen van onze versnellingsmeter
Rgyro - Rest(n-1) en huidige gyroscoop lezingen verkregen
Welke waarden gebruiken we voor het berekenen van de geactualiseerde raming Rest(n)? U waarschijnlijk geraden dat we beide zullen gebruiken. We zullen gebruik maken van een gewogen gemiddelde, zodat:
Rest(n) = (Racc * w1 + Rgyro * w2) / (w1 + w2)
We kunnen deze formule vereenvoudigen door zowel de teller en de noemer van de breuk delen door w1.
Rest(n) = (Racc * w1/w1 + Rgyro * w2/w1) / (w1/w1 + w2/w1)
en na vervanging van w2/w1 = wGyro die we krijgen:
Rest(n) = (Racc + Rgyro * wGyro) / (1 + wGyro)
In de bovenstaande Formula vertelt wGyro ons hoeveel wij vertrouwen erop dat onze gyro dat ten opzichte van onze versnellingsmeter. Deze waarde kan meestal experimenteel worden gekozen waarden tussen 5..20 zal leiden tot goede resultaten.
Het belangrijkste verschil van dit algoritme van Kalman-filter is dat dit gewicht relatief vast is, terwijl in Kalman filter die de gewichten worden permanent bijgewerkt op basis van het gemeten geluid van de lezingen van de versnellingsmeter. Kalman-filter is gericht op het geven van u "het beste" theoretische resultaten, terwijl dit algoritme u resultaten "good enough" voor uw praktische toepassing geven kan. U kunt het implementeren van een algoritme dat wGyro afhankelijk van enkele factoren ruis die u meten past, maar vaste waarden zal goed werken voor de meeste toepassingen.
Wij zijn een stap verwijderd van onze bijgewerkte geschatte waarden krijgen:
RxEst(n) = (RxAcc + RxGyro * wGyro) / (1 + wGyro)
RyEst(n) = (RyAcc + RyGyro * wGyro) / (1 + wGyro)
RzEst(n) = (RzAcc + RzGyro * wGyro) / (1 + wGyro)
Nu laten we weer normaliseren deze vector:
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
En we zijn klaar voor onze lus te herhalen.
Deze handleiding oorspronkelijk verscheen op starlino.com, ik heb een paar lichte bewerkingen gemaakt en opnieuw gepost met toestemming. Bedankt Starlino!
