Samouczek dotyczący akcelerometru i żyroskopu: 3 kroki

2024 Autor: John Day | [email protected]. Ostatnio zmodyfikowany: 2024-01-30 11:34

Wstęp

Ten przewodnik jest przeznaczony dla wszystkich zainteresowanych wykorzystaniem akcelerometrów i żyroskopów oraz kombinacji urządzeń IMU (Inertial Measurement Unit) w swoich projektach elektronicznych

Omówimy:

• Co mierzy akcelerometr?
• Co mierzy żyroskop (inaczej żyroskop)?
• Jak przekonwertować odczyty analogowo-cyfrowe (ADC) otrzymywane z tych czujników na jednostki fizyczne (g dla akcelerometru, deg/s dla żyroskopu)
• Jak połączyć odczyty akcelerometru i żyroskopu, aby uzyskać dokładne informacje o nachyleniu urządzenia względem płaszczyzny uziemienia

W całym artykule postaram się ograniczyć matematykę do minimum. Jeśli wiesz, czym są Sine/Cosinus/Tangent, powinieneś być w stanie zrozumieć i wykorzystać te pomysły w swoim projekcie, niezależnie od używanej platformy: Arduino, Propeller, Basic Stamp, chipy Atmel, Microchip PIC itp.

Są ludzie, którzy uważają, że potrzebujesz skomplikowanej matematyki, aby korzystać z jednostki IMU (złożone filtry FIR lub IIR, takie jak filtry Kalmana, filtry Parksa-McClellana itp.). Możesz je wszystkie zbadać i osiągnąć wspaniałe, ale złożone rezultaty. Mój sposób wyjaśniania rzeczy wymaga tylko podstawowej matematyki. Jestem wielkim wyznawcą prostoty. Myślę, że system, który jest prosty jest łatwiejszy do kontrolowania i monitorowania, poza tym wiele urządzeń wbudowanych nie ma mocy i zasobów do implementacji skomplikowanych algorytmów wymagających obliczeń macierzowych.

Jako przykładu posłużę się nową jednostką IMU, Acc_Gyro Accelerometer + Gyro IMU. W poniższych przykładach użyjemy parametrów tego urządzenia. Ta jednostka jest dobrym urządzeniem na początek, ponieważ składa się z 2 urządzeń:

- LIS331AL (karta katalogowa) - trójosiowy akcelerometr 2G - LPR550AL (karta katalogowa) - dwuosiowy pitch and roll, żyroskop 500 st/s

Razem reprezentują one inercyjną jednostkę miary 5 stopni swobody. To wymyślna nazwa! Niemniej jednak za wymyślną nazwą kryje się bardzo przydatne połączenie, które omówimy i szczegółowo wyjaśnimy w tym przewodniku.

Krok 1: Akcelerometr

Aby zrozumieć tę jednostkę, zaczniemy od akcelerometru. Myśląc o akcelerometrach często warto wyobrazić sobie pudełko w kształcie sześcianu z kulką w środku. Możesz sobie wyobrazić coś innego, jak ciastko lub pączek, ale ja wyobrazę sobie piłkę:

Jeśli weźmiemy to pudełko w miejsce, w którym nie ma pól grawitacyjnych lub innych pól, które mogłyby wpłynąć na pozycję piłki - piłka po prostu unosi się na środku pudełka. Możesz sobie wyobrazić, że pudełko znajduje się w przestrzeni kosmicznej, daleko od wszelkich ciał kosmicznych, a jeśli takie miejsce jest trudne do znalezienia, wyobraź sobie przynajmniej statek kosmiczny krążący wokół planety, gdzie wszystko jest w stanie nieważkości. Na powyższym obrazku widać, że do każdej osi przypisujemy parę ścian (usunęliśmy ścianę Y+, abyśmy mogli zajrzeć do wnętrza pudełka). Wyobraź sobie, że każda ściana jest wrażliwa na nacisk. Jeśli nagle przesuniemy pudło w lewo (przyspieszamy je z przyspieszeniem 1g = 9,8m/s^2), piłka uderzy w ścianę X-. Następnie mierzymy siłę nacisku, jaką kula przykłada do ściany i wyświetlamy wartość -1g na osi X.

Należy pamiętać, że akcelerometr faktycznie wykryje siłę skierowaną w kierunku przeciwnym do wektora przyspieszenia. Siła ta jest często nazywana siłą bezwładności lub siłą fikcyjną. Jedną z rzeczy, których powinieneś się z tego nauczyć, jest to, że akcelerometr mierzy przyspieszenie pośrednio poprzez siłę przyłożoną do jednej ze ścian (zgodnie z naszym modelem może to być sprężyna lub coś innego w rzeczywistych akcelerometrach). Siła ta może być spowodowana przyspieszeniem, ale jak zobaczymy w następnym przykładzie, nie zawsze jest spowodowana przyspieszeniem.

Jeśli weźmiemy nasz model i umieścimy go na Ziemi, kulka spadnie na ścianę Z i przyłoży siłę 1 g na dolną ścianę, jak pokazano na poniższym obrazku:

W tym przypadku pudełko się nie porusza, ale nadal otrzymujemy odczyt -1g na osi Z. Nacisk wywierany przez kulę na ścianę był spowodowany siłą grawitacji. Teoretycznie może to być inny rodzaj siły - na przykład, jeśli wyobrazisz sobie, że nasza kulka jest metalowa, umieszczenie magnesu obok pudełka może przesunąć kulkę tak, aby uderzyła w inną ścianę. Zostało to powiedziane tylko po to, aby udowodnić, że w istocie akcelerometr mierzy siłę, a nie przyspieszenie. Po prostu zdarza się, że przyspieszenie powoduje siłę bezwładności, która jest wychwytywana przez mechanizm wykrywania siły akcelerometru.

Chociaż ten model nie jest dokładnie taki, jak skonstruowany jest czujnik MEMS, często jest przydatny w rozwiązywaniu problemów związanych z akcelerometrem. W rzeczywistości istnieją podobne czujniki, które mają metalowe kulki w środku, nazywane są przełącznikami przechyłu, jednak są one bardziej prymitywne i zwykle potrafią tylko stwierdzić, czy urządzenie jest nachylone w pewnym zakresie, czy nie, a nie zakres nachylenia.

Do tej pory analizowaliśmy dane wyjściowe akcelerometru na jednej osi i to wszystko, co uzyskasz dzięki akcelerometrom jednoosiowym. Rzeczywista wartość akcelerometrów trójosiowych wynika z faktu, że mogą wykrywać siły bezwładności na wszystkich trzech osiach. Wróćmy do naszego modelu pudełka i obróćmy pudełko o 45 stopni w prawo. Piłka dotknie teraz 2 ścian: Z- i X-, jak pokazano na poniższym obrazku:

Wartości 0,71 nie są arbitralne, w rzeczywistości są przybliżeniem dla SQRT(1/2). Stanie się to jaśniejsze, gdy wprowadzimy nasz kolejny model akcelerometru.

W poprzednim modelu naprawiliśmy siłę grawitacji i obróciliśmy nasze wyimaginowane pudełko. W ostatnich 2 przykładach przeanalizowaliśmy dane wyjściowe w 2 różnych pozycjach pudełka, podczas gdy wektor siły pozostał stały. Chociaż było to przydatne w zrozumieniu, w jaki sposób akcelerometr oddziałuje z siłami zewnętrznymi, bardziej praktyczne jest wykonywanie obliczeń, jeśli przymocujemy układ współrzędnych do osi akcelerometru i wyobrazimy sobie, że wektor siły obraca się wokół nas.

Proszę spojrzeć na powyższy model, zachowałem kolory osi, aby można było mentalnie przejść z poprzedniego modelu do nowego. Wyobraź sobie, że każda oś w nowym modelu jest prostopadła do odpowiednich ścian pudełka w poprzednim modelu. Wektor R jest wektorem siły mierzonej przez akcelerometr (może to być siła grawitacji lub siła bezwładności z powyższych przykładów lub kombinacja obu). Rx, Ry, Rz to rzut wektora R na osie X, Y, Z. Proszę zwrócić uwagę na następującą zależność:

R^2 = Rx^2 + Ry^2 + Rz^2 (Równanie 1)

co jest w zasadzie odpowiednikiem twierdzenia Pitagorasa w 3D.

Pamiętaj, że nieco wcześniej powiedziałem Ci, że wartości SQRT(1/2) ~ 0,71 nie są losowe. Jeśli wstawisz je do powyższego wzoru, po przypomnieniu, że nasza siła grawitacji wynosiła 1 g, możemy zweryfikować, że:

1^2 = (-PIERWIASTEK(1/2))^2 + 0 ^2 + (-PIERWIASTEK(1/2))^2

po prostu zastępując R=1, Rx = -SQRT(1/2), Ry = 0, Rz = -SQRT(1/2) w równaniu 1

Po długiej preambule teorii zbliżamy się do prawdziwych akcelerometrów. Wartości Rx, Ry, Rz są w rzeczywistości liniowo powiązane z wartościami, które wygeneruje Twój rzeczywisty akcelerometr i których możesz użyć do wykonywania różnych obliczeń.

Zanim tam dotrzemy, porozmawiajmy trochę o tym, w jaki sposób akcelerometry będą nam dostarczać te informacje. Większość akcelerometrów można podzielić na dwie kategorie: cyfrowe i analogowe. Akcelerometry cyfrowe podadzą informacje za pomocą protokołu szeregowego, takiego jak I2C, SPI lub USART, podczas gdy akcelerometry analogowe wyprowadzą poziom napięcia w określonym zakresie, który należy przekonwertować na wartość cyfrową za pomocą modułu ADC (przetwornika analogowo-cyfrowego). Nie będę wdawał się zbytnio w to, jak działa ADC, częściowo dlatego, że jest to tak obszerny temat, a częściowo dlatego, że różni się w zależności od platformy. Niektóre mikrokontrolery będą miały wbudowane moduły ADC, niektóre z nich będą wymagały zewnętrznych komponentów w celu wykonania konwersji ADC. Bez względu na to, jakiego typu modułu ADC używasz, otrzymasz wartość z określonego zakresu. Na przykład 10-bitowy moduł ADC wygeneruje wartość z zakresu 0..1023, zauważ, że 1023 = 2^10 -1. 12-bitowy moduł ADC wyprowadzi wartość z zakresu 0..4095, zauważ, że 4095 = 2^12-1.

Przejdźmy dalej, rozważając prosty przykład, załóżmy, że nasz 10-bitowy moduł ADC dał nam następujące wartości dla trzech kanałów akcelerometru (osie):

AdcRx = 586 AdcRy = 630 AdcRz = 561

Każdy moduł ADC będzie miał napięcie odniesienia, załóżmy w naszym przykładzie jest to 3,3V. Aby przekonwertować 10-bitową wartość ADC na napięcie, używamy następującego wzoru:

VoltsRx = AdcRx * Vref / 1023

Szybka uwaga: dla 8-bitowego ADC ostatnim dzielnikiem będzie 255 = 2 ^ 8 -1, a dla 12-bitowego ADC ostatnim dzielnikiem będzie 4095 = 2 ^ 12 -1.

Stosując tę formułę do wszystkich 3 kanałów otrzymujemy:

VoltsRx = 586 * 3,3 V / 1023 = ~ 1,89 V (wszystkie wyniki zaokrąglamy do 2 miejsc po przecinku) VoltsRy = 630 * 3,3 V / 1023 =~ 2,03 V Volts Rz = 561 * 3,3 V / 1023 = ~ 1,81 V

Każdy akcelerometr ma poziom napięcia zero-g, można go znaleźć w specyfikacji, jest to napięcie odpowiadające 0g. Aby otrzymać wartość napięcia ze znakiem musimy obliczyć przesunięcie od tego poziomu. Powiedzmy, że nasz poziom napięcia 0g wynosi VzeroG = 1,65V. Przesunięcia napięcia od napięcia zerowego obliczamy w następujący sposób:

DeltaVoltsRx = 1,89V - 1,65V = 0,24V DeltaVoltsRy = 2,03V - 1,65V = 0,38V DeltaVoltsRz = 1,81V - 1,65V = 0,16V

Mamy teraz odczyty akcelerometru w woltach, nadal nie jest w g (9,8 m/s^2), aby dokonać ostatecznej konwersji, stosujemy czułość akcelerometru, zwykle wyrażoną w mV/g. Powiedzmy, że nasza czułość = 478,5mV/g = 0,4785V/g. Wartości czułości można znaleźć w specyfikacjach akcelerometru. Aby otrzymać końcowe wartości siły wyrażone w g, korzystamy z następującego wzoru:

Rx = DeltaVoltsRx / czułość

Rx = 0,24V / 0,4785V/g =~0,5g Ry = 0,38V / 0,4785V/g =~0,79g Rz = 0,16V / 0,4785V/g =~0,33g

Moglibyśmy oczywiście połączyć wszystkie kroki w jedną formułę, ale przeszedłem przez wszystkie kroki, aby wyjaśnić, w jaki sposób przechodzisz od odczytów ADC do składowej wektora siły wyrażonej w g.

Rx = (AdcRx * Vref / 1023 - VzeroG) / Czułość (Równanie 2) Ry = (AdcRy * Vref / 1023 - VzeroG) / Czułość Rz = (AdcRz * Vref / 1023 - VzeroG) / Czułość

Mamy teraz wszystkie 3 komponenty, które definiują nasz wektor siły bezwładności, jeśli urządzenie nie podlega innym siłom niż grawitacja, możemy założyć, że jest to kierunek naszego wektora siły grawitacji. Jeśli chcesz obliczyć nachylenie urządzenia względem podłoża, możesz obliczyć kąt między tym wektorem a osią Z. Jeśli interesuje Cię również kierunek nachylenia na oś, możesz podzielić ten wynik na 2 składowe: nachylenie na osi X i Y, które można obliczyć jako kąt między wektorem grawitacji a osiami X / Y. Obliczenie tych kątów jest prostsze, niż mogłoby się wydawać, teraz, gdy obliczyliśmy wartości dla Rx, Ry i Rz. Wróćmy do naszego ostatniego modelu akcelerometru i zróbmy kilka dodatkowych zapisów:

Kąty, które nas interesują, to kąty pomiędzy osiami X, Y, Z i wektorem siły R. Zdefiniujemy te kąty jako Axr, Ayr, Azr. Z trójkąta prostokątnego utworzonego przez R i Rx można zauważyć, że:

cos(Axr) = Rx / R i podobnie: cos(Ayr) = Ry / R cos(Azr) = Rz / R

Z równania 1 możemy wywnioskować, że R = SQRT(Rx^2 + Ry^2 + Rz^2).

Możemy teraz znaleźć nasze kąty za pomocą funkcji arccos() (funkcja odwrotna cos()):

Axr = arccos(Rx/R) Ayr = arccos(Ry/R) Azr = arccos(Rz/R)

Przeszliśmy długą drogę, aby wyjaśnić model akcelerometru, żeby dojść do tych wzorów. W zależności od aplikacji możesz użyć dowolnych formuł pośrednich, które wyprowadziliśmy. Wkrótce przedstawimy również model żyroskopowy i zobaczymy, jak można połączyć dane z akcelerometru i żyroskopu, aby zapewnić jeszcze dokładniejsze szacunki nachylenia.

Ale zanim to zrobimy, zróbmy kilka bardziej przydatnych notacji:

cosX = cos(Axr) = Rx / R cosy = cos(Ayr) = Ry / R cosZ = cos(Azr) = Rz / R

Ten tryplet jest często nazywany Direction Cosinus i zasadniczo reprezentuje wektor jednostkowy (wektor o długości 1), który ma ten sam kierunek co nasz wektor R. Możesz łatwo zweryfikować, że:

PIERWIASTEK(cosX^2 + cosY^2 + cosZ^2) = 1

Jest to fajna właściwość, ponieważ zwalnia nas z monitorowania modułu (długości) wektora R. Często, jeśli interesuje nas tylko kierunek naszego wektora bezwładności, warto znormalizować jego moduł w celu uproszczenia innych obliczeń.

Krok 2: Żyroskop

Nie zamierzamy wprowadzać żadnego równoważnego modelu pudełkowego dla żyroskopu, jak to zrobiliśmy dla akcelerometru, zamiast tego przejdziemy od razu do drugiego modelu akcelerometru i pokażemy, co mierzy żyroskop zgodnie z tym modelem.

Każdy kanał żyroskopu mierzy obrót wokół jednej z osi. Na przykład żyroskop 2-osiowy mierzy obrót wokół (lub niektórzy mogą powiedzieć „o”) osi X i Y. Aby wyrazić tę rotację w liczbach, zróbmy kilka notacji. Najpierw zdefiniujmy:

Rxz - jest rzutem wektora siły bezwładności R na płaszczyznę XZ Ryz - jest rzutem wektora siły bezwładności R na płaszczyznę YZ

Z trójkąta prostokątnego utworzonego przez Rxz i Rz, korzystając z twierdzenia Pitagorasa otrzymujemy:

Rxz^2 = Rx^2 + Rz^2 i podobnie: Ryz^2 = Ry^2 + Rz^2

zauważ również, że:

R^2 = Rxz^2 + Ry^2, można to wyprowadzić z równania 1 i wyższych, lub z trójkąta prostokątnego utworzonego przez R i Ryz R^2 = Ryz^2 + Rx^2

Nie będziemy używać tych formuł w tym artykule, ale warto zwrócić uwagę na związek między wszystkimi wartościami w naszym modelu.

Zamiast tego zdefiniujemy kąt między osią Z a wektorami Rxz, Ryz w następujący sposób:

Axz - to kąt pomiędzy Rxz (rzut R na płaszczyznę XZ) a osią Z Ayz - to kąt pomiędzy Ryz (rzut R na płaszczyznę YZ) a osią Z

Teraz zbliżamy się do tego, co mierzy żyroskop. Żyroskop mierzy szybkość zmian kątów zdefiniowanych powyżej. Innymi słowy, wygeneruje wartość, która jest liniowo powiązana z szybkością zmiany tych kątów. Aby to wyjaśnić załóżmy, że zmierzyliśmy kąt obrotu wokół osi Y (to byłby kąt Axz) w czasie t0 i definiujemy go jako Axz0, następnie zmierzyliśmy ten kąt w późniejszym czasie t1 i było to Axz1. Tempo zmian będzie obliczane w następujący sposób:

RateAxz = (Axz1 - Axz0) / (t1 - t0).

Jeśli wyrazimy Axz w stopniach, a czas w sekundach, to ta wartość będzie wyrażona w deg/s. To właśnie mierzy żyroskop.

W praktyce żyroskop (chyba, że jest to specjalny żyroskop cyfrowy) rzadko podaje wartość wyrażoną w stopniach/s. Tak samo jak w przypadku akcelerometru, otrzymasz wartość ADC, którą musisz przekonwertować na stopnie/s za pomocą wzoru podobnego do równania. 2, które zdefiniowaliśmy dla akcelerometru. Przedstawmy wzór konwersji ADC na deg/s dla żyroskopu (zakładamy, że używamy modułu 10bit ADC, dla 8bit ADC zamień 1023 na 255, dla 12bit ADC zamień 1023 na 4095).

RateAxz = (AdcGyroXZ * Vref / 1023 - VzeroRate) / Równanie czułości 3 RateAyz = (AdcGyroYZ * Vref / 1023 - VzeroRate) / czułość

AdcGyroXZ, AdcGyroYZ - są uzyskiwane z naszego modułu adc i reprezentują kanały, które mierzą obrót rzutu wektora R w XZ odpowiednio w płaszczyznach YZ, co jest równoważne powiedzeniu, że obrót został wykonany odpowiednio wokół osi Y i X.

Vref - jest napięciem odniesienia ADC, którego użyjemy w poniższym przykładzie. na przykład 1,23V (możesz znaleźć te wartości w specyfikacji) Czułość - to czułość twojego żyroskopu wyrażona w mV / (deg / s) często zapisywana jako mV/deg/s, zasadniczo mówi ci, ile mV będzie wyjście żyroskopu wzrasta, jeśli zwiększysz prędkość obrotową o jeden stopień/s. Czułość płytki Acc_Gyro wynosi np. 2mV/deg/s lub 0,002V/deg/s

Weźmy przykład, załóżmy, że nasz moduł ADC zwrócił następujące wartości:

AdcGyroXZ = 571 AdcGyroXZ = 323

Korzystając z powyższego wzoru i korzystając z parametrów specyfikacji płyty Acc_Gyro otrzymamy:

RateAxz = (571 * 3,3V / 1023 - 1,23V) / (0,002V/st/s) =~ 306 stopni/s RateAyz = (323 * 3,3V / 1023 - 1,23V) / (0,002V/st/s) =~ -94 st./s

Innymi słowy urządzenie obraca się wokół osi Y (lub możemy powiedzieć, że obraca się w płaszczyźnie XZ) z prędkością 306 st./s i wokół osi X (lub możemy powiedzieć, że obraca się w płaszczyźnie YZ) z prędkością - 94 stopnie/s. Należy pamiętać, że znak minus oznacza, że urządzenie obraca się w kierunku przeciwnym do konwencjonalnego kierunku dodatniego. Zgodnie z konwencją jeden kierunek obrotu jest dodatni. Dobry arkusz specyfikacji żyroskopu pokaże, który kierunek jest dodatni, w przeciwnym razie będziesz musiał go znaleźć, eksperymentując z urządzeniem i zauważając, który kierunek obrotu powoduje wzrost napięcia na pinie wyjściowym. Najlepiej zrobić to za pomocą oscyloskopu, ponieważ jak tylko zatrzymasz obrót, napięcie spadnie z powrotem do poziomu zerowego. Jeśli używasz multimetru, musisz utrzymać stałą prędkość obrotową przez co najmniej kilka sekund i zanotować napięcie podczas tego obrotu, a następnie porównać je z napięciem zerowym. Jeśli jest większe od napięcia zerowego, oznacza to, że kierunek obrotów jest dodatni.

Krok 3: Połączenie akcelerometru i żyroskopu

Łącząc to wszystko w jedną całość - Łączenie danych z akcelerometru i żyroskopu

Jeśli czytasz ten artykuł, prawdopodobnie nabyłeś lub planujesz nabyć urządzenie IMU, lub prawdopodobnie planujesz zbudować je z oddzielnych urządzeń akcelerometru i żyroskopu.

Pierwszym krokiem w stosowaniu połączonego urządzenia IMU, które łączy akcelerometr i żyroskop, jest wyrównanie ich układów współrzędnych. Najłatwiej to zrobić, wybierając układ współrzędnych akcelerometru jako referencyjny układ współrzędnych. Większość arkuszy danych akcelerometru wyświetla kierunek osi X, Y, Z względem obrazu fizycznego chipa lub urządzenia. Na przykład tutaj są kierunki osi X, Y, Z, jak pokazano w specyfikacji karty Acc_Gyro:

Kolejne kroki to:

Zidentyfikuj wyjścia żyroskopu, które odpowiadają wartościom RateAxz, RateAyz omówionym powyżej. Określ, czy te wyjścia muszą być odwrócone ze względu na fizyczne położenie żyroskopu względem akcelerometru

Nie zakładaj, że jeśli żyroskop ma wyjście oznaczone X lub Y, będzie ono odpowiadać dowolnej osi układu współrzędnych akcelerometru, nawet jeśli to wyjście jest częścią jednostki IMU. Najlepiej to przetestować. Zakładając, że ustaliłeś położenie żyroskopu względem akcelerometru. Zakłada się, że granice żyroskopu i akcelerometru są równoległe do siebie, tj. umieszczasz żyroskop pod kątem wielokrotności 90 stopni względem chipa akcelerometru. Jeśli kupiłeś tablicę IMU, są szanse, że są one już wyrównane w ten sposób. W tym artykule nie będziemy omawiać modeli, w których żyroskop jest umieszczony pod nieregularnym kątem względem akcelerometru (powiedzmy 45 lub 30 stopni), chociaż może to być przydatne w niektórych aplikacjach.

Oto przykładowa sekwencja określająca, które wyjście żyroskopu odpowiada omówionej powyżej wartości RateAxz.

- zacznij od ustawienia urządzenia w pozycji poziomej. Zarówno wyjścia X, jak i Y akcelerometru wyprowadzają napięcie zerowe (na przykład dla płyty Acc_Gyro jest to 1,65 V)

- następnie zacznij obracać urządzenie wokół osi Y, inaczej można powiedzieć, że obracasz urządzenie w płaszczyźnie XZ, tak że wyjścia akcelerometru X i Z zmieniają się, a wyjście Y pozostaje stałe. - obracając urządzenie ze stałą prędkością zanotuj, które wyjście żyroskopu się zmienia, pozostałe wyjścia żyroskopu powinny pozostać stałe - wyjście żyroskopu, które zmieniło się podczas obrotu wokół osi Y (obrót w płaszczyźnie XZ) będzie stanowić wartość wejściową dla AdcGyroXZ, z której obliczamy RateAxz - ostatnim krokiem jest upewnienie się, że kierunek obrotu jest zgodny z naszym modelem, w niektórych przypadkach może być konieczne odwrócenie wartości RateAxz ze względu na fizyczne położenie żyroskopu względem akcelerometru - ponownie wykonaj powyższy test, obracając urządzenie dookoła oś Y, tym razem monitoruj wyjście X akcelerometru (AdcRx w naszym modelu). Jeśli AdcRx rośnie (pierwsze 90 stopni obrotu od pozycji poziomej), to AdcGyroXZ również powinien rosnąć. W przeciwnym razie musisz odwrócić RateAxz, możesz to osiągnąć, wprowadzając współczynnik znaku w równaniu 3 w następujący sposób:

RateAxz = InvertAxz * (AdcGyroXZ * Vref/1023 - VzeroRate) / Czułość, gdzie InvertAxz wynosi 1 lub -1

ten sam test można wykonać dla RateAyz, obracając urządzenie wokół osi X, i można określić, które wyjście żyroskopu odpowiada RateAyz i czy należy je odwrócić. Po uzyskaniu wartości InvertAyz należy użyć następującego wzoru do obliczenia RateAyz:

RateAyz = InvertAyz * (AdcGyroYZ * Vref / 1023 - VzeroRate) / Czułość

Jeśli wykonałbyś te testy na płytce Acc_Gyro, otrzymasz następujące wyniki:

- pin wyjściowy dla RateAxz to GX4 i InvertAxz = -1. - pin wyjściowy dla RateAyz to GY4 a InvertAyz = -1

Od tego momentu będziemy uważać, że skonfigurowałeś swój IMU w taki sposób, aby można było obliczyć prawidłowe wartości dla Axr, Ayr, Azr (zgodnie z częścią 1. Akcelerometr) i RateAxz, RateAyz (zgodnie z częścią 2. Żyroskop). Następnie przeanalizujemy relacje między tymi wartościami, które okazują się przydatne w uzyskaniu dokładniejszego oszacowania nachylenia urządzenia względem płaszczyzny podłoża.

W tym momencie możesz zadać sobie pytanie, czy model akcelerometru podał nam już kąty nachylenia Axr, Ayr, Azr, po co mielibyśmy zawracać sobie głowę danymi z żyroskopu? Odpowiedź jest prosta: nie zawsze można w 100% ufać danym akcelerometru. Powodów jest kilka, pamiętaj, że akcelerometr mierzy siłę bezwładności, taka siła może być spowodowana grawitacją (a najlepiej tylko grawitacją), ale może też być spowodowana przyspieszeniem (ruchem) urządzenia. W rezultacie, nawet jeśli akcelerometr jest w stosunkowo stabilnym stanie, nadal jest bardzo wrażliwy na wibracje i ogólnie hałas mechaniczny. Jest to główny powód, dla którego większość systemów IMU używa żyroskopu do wygładzania wszelkich błędów akcelerometru. Ale jak to się robi? A czy żyroskop jest wolny od szumów ?

Żyroskop nie jest wolny od szumów, jednak ponieważ mierzy obrót, jest mniej wrażliwy na liniowe ruchy mechaniczne, rodzaj szumu, na który cierpi akcelerometr, jednak żyroskopy mają inne problemy, takie jak na przykład dryf (nie powraca do wartości zerowej gdy rotacja się zatrzyma). Niemniej jednak, uśredniając dane pochodzące z akcelerometru i żyroskopu, możemy uzyskać stosunkowo lepsze oszacowanie aktualnego nachylenia urządzenia, niż uzyskalibyśmy przy użyciu samych danych akcelerometru.

W kolejnych krokach przedstawię algorytm, który został zainspirowany niektórymi pomysłami zastosowanymi w filtrze Kalmana, jednak jest zdecydowanie prostszy i łatwiejszy do zaimplementowania na urządzeniach wbudowanych. Zanim to nastąpi, zobaczmy najpierw, co nasz algorytm ma obliczyć. Cóż, to kierunek wektora siły grawitacji R = [Rx, Ry, Rz], z którego możemy wyprowadzić inne wartości, takie jak Axr, Ayr, Azr lub cosX, cosY, cosZ, da nam wyobrażenie o nachyleniu naszego urządzenia względem płaszczyzny podłoża omawiamy zależność między tymi wartościami w Części 1. Można by powiedzieć - czy nie mamy już tych wartości Rx, Ry, Rz z równania 2 w Części 1 ? Cóż, tak, ale pamiętaj, że te wartości pochodzą wyłącznie z danych akcelerometru, więc jeśli będziesz ich używać bezpośrednio w swojej aplikacji, możesz uzyskać więcej szumu niż Twoja aplikacja może tolerować. Aby uniknąć dalszych nieporozumień, zdefiniujmy na nowo pomiary akcelerometru w następujący sposób:

Racc - to wektor siły bezwładności mierzony akcelerometrem, składający się z następujących składowych (rzutów na osie X, Y, Z):

RxAcc = (AdcRx * Vref / 1023 - VzeroG) / Czułość RyAcc = (AdcRy * Vref / 1023 - VzeroG) / Czułość RzAcc = (AdcRz * Vref / 1023 - VzeroG) / Czułość

Do tej pory dysponujemy zbiorem zmierzonych wartości, które możemy uzyskać wyłącznie z wartości ADC akcelerometru. Nazwiemy ten zestaw danych „wektorem” i użyjemy następującej notacji.

Racc = [RxAcc, RyAcc, RzAcc]

Ponieważ te składniki Racc można uzyskać z danych akcelerometru, możemy uznać je za dane wejściowe do naszego algorytmu.

Zwróć uwagę, że ponieważ Racc mierzy siłę grawitacji, będziesz miał rację, jeśli założysz, że długość tego wektora, zdefiniowanego w następujący sposób, jest równa lub bliska 1g.

|Racc| = PIERWIASTEK(RxAcc^2 +RyAcc^2 + RzAcc^2), Aby jednak mieć pewność, warto zaktualizować ten wektor w następujący sposób:

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

Zapewni to, że długość znormalizowanego wektora Racc wynosi zawsze 1.

Następnie wprowadzimy nowy wektor i nazwiemy go

Reszta = [RxEst, RyEst, RzEst]

Będzie to wynik naszego algorytmu, są to skorygowane wartości oparte na danych z żyroskopu i na podstawie danych szacunkowych z przeszłości.

Oto, co zrobi nasz algorytm: - akcelerometr mówi nam: "Jesteś teraz na pozycji Racc" - mówimy "Dziękuję, ale pozwól mi sprawdzić", - następnie popraw tę informację za pomocą danych z żyroskopu, a także danych z poprzedniego spoczynku i wyprowadzamy nowy oszacowany wektor Rest. - uważamy Rest za nasz "najlepszy zakład" co do aktualnej pozycji urządzenia.

Zobaczmy, jak możemy to zrobić.

Sekwencję rozpoczniemy od zaufania naszemu akcelerometrowi i przypisania:

Reszta (0) = Racc (0)

Przy okazji pamiętaj, że Rest i Racc są wektorami, więc powyższe równanie jest po prostu prostym sposobem na napisanie 3 zestawów równań i uniknięcie powtórzeń:

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

Następnie wykonamy regularne pomiary w równych odstępach czasu co T sekund i otrzymamy nowe pomiary, które zdefiniujemy jako Racc(1), Racc(2), Racc(3) i tak dalej. Będziemy również wydawać nowe szacunki w każdym przedziale czasowym Rest(1), Rest(2), Rest(3) i tak dalej.

Załóżmy, że jesteśmy w kroku n. Mamy dwa znane zestawy wartości, których chcielibyśmy użyć:

Rest(n-1) - nasze poprzednie oszacowanie, gdzie Rest(0) = Racc(0) Racc(n) - nasz aktualny pomiar akcelerometru

Zanim będziemy mogli obliczyć Rest(n), wprowadźmy nową zmierzoną wartość, którą możemy uzyskać z naszego żyroskopu i poprzednie oszacowanie.

Nazwiemy go Rgyro i jest to również wektor składający się z 3 elementów:

Rżyr = [RxGyro, RyGyro, RzGyro]

Obliczymy ten wektor po jednym składniku na raz. Zaczniemy od RxGyro.

Zacznijmy od obserwacji następującej zależności w naszym modelu żyroskopu, z trójkąta prostokątnego utworzonego przez Rz i Rxz możemy wyprowadzić, że:

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

Atan2 może być funkcją, której nigdy wcześniej nie używałeś, jest podobna do atan, z wyjątkiem tego, że zwraca wartości z zakresu (-PI, PI) w przeciwieństwie do (-PI/2, PI/2) zwracanych przez atan i 2 argumenty zamiast jednego. Pozwala na konwersję dwóch wartości Rx, Rz na kąty w pełnym zakresie 360 stopni (-PI do PI). Więcej o atan2 przeczytasz tutaj.

Znając RxEst(n-1) i RzEst(n-1) możemy znaleźć:

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

Pamiętaj, że żyroskop mierzy szybkość zmian kąta Axz. Zatem możemy oszacować nowy kąt Axz(n) w następujący sposób:

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

Pamiętaj, że RateAxz można uzyskać z naszych odczytów ADC żyroskopu. Bardziej precyzyjny wzór może posłużyć się średnią rotacją obliczoną w następujący sposób:

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

W ten sam sposób możemy znaleźć:

Ayz(n) = Ayz(n-1) + WspółczynnikAyz(n) * T

Ok, więc teraz mamy Axz(n) i Ayz(n). Gdzie idziemy stąd, aby odliczyć RxGyro/RyGyro? Z równania 1 możemy zapisać długość wektora Rgyro w następujący sposób:

|Żyroskop| = PIERWIASTEK(RxGyro^2 + RyGyro^2 + RzGyro^2)

Ponieważ znormalizowaliśmy nasz wektor Racc, możemy założyć, że ma on długość 1 i nie zmienił się po obrocie, więc stosunkowo bezpiecznie jest napisać:

|Żyroskop| = 1

Przyjmijmy tymczasową krótszą notację do poniższych obliczeń:

x =RxŻyro, y=Żyro, z=RzŻyro

Korzystając z powyższych relacji możemy napisać:

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

Podzielmy licznik i mianownik ułamka przez SQRT(x^2 + z^2)

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

Zauważ, że x / SQRT(x^2 + z^2) = sin(Axz), więc:

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

Teraz pomnóż licznik i mianownik ułamka wewnątrz SQRT przez z^2

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

Zauważ, że z / SQRT(x^2 + z^2) = cos(Axz) i y / z = tan(Ayz), więc w końcu:

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

Wracając do naszej notacji, otrzymujemy:

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

w ten sam sposób, w jaki to znajdujemy

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

Teraz w końcu możemy znaleźć:

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

Gdzie Sign(RzGyro) = 1, gdy RzGyro>=0 i Sign(RzGyro) = -1, gdy RzGyro<0.

Jednym prostym sposobem na oszacowanie tego jest przyjęcie:

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

W praktyce należy zachować ostrożność, gdy RzEst(n-1) jest bliskie 0. W tym przypadku można całkowicie pominąć fazę żyroskopu i przypisać: Rgyro = Rest(n-1). Rz służy jako odniesienie do obliczania kątów Axz i Ayz, a gdy jest bliskie 0, wartości mogą przekroczyć i spowodować złe wyniki. Będziesz w dziedzinie dużych liczb zmiennoprzecinkowych, w których implementacje funkcji tan() / atan() mogą nie być precyzyjne.

Podsumujmy więc to, co mamy do tej pory, jesteśmy w kroku n naszego algorytmu i obliczyliśmy następujące wartości:

Racc - aktualne odczyty z naszego akcelerometru Rgyro - uzyskane z Rest(n-1) oraz aktualne odczyty żyroskopu

Jakich wartości używamy do obliczenia zaktualizowanego oszacowania Rest(n) ? Prawdopodobnie zgadłeś, że użyjemy obu. Użyjemy średniej ważonej, aby:

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

Możemy uprościć tę formułę, dzieląc licznik i mianownik ułamka przez w1.

Reszta(n) = (Rcc * w1/w1 + Rgyro * w2/w1) / (w1/w1 + w2/w1)

a po podstawieniu w2/w1 = wGyro otrzymujemy:

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

Na powyższym forum wGyro mówi nam, jak bardzo ufamy naszemu żyroskopowi w porównaniu do naszego akcelerometru. Wartość tę można dobrać eksperymentalnie, zwykle dobre wyniki dadzą wartości pomiędzy 5..20.

Główną różnicą tego algorytmu od filtru Kalmana jest to, że waga ta jest względnie stała, podczas gdy w filtrze Kalmana wagi są stale aktualizowane na podstawie zmierzonego szumu odczytów akcelerometru. Filtr Kalmana koncentruje się na zapewnieniu „najlepszych” wyników teoretycznych, podczas gdy ten algorytm może dać wyniki „wystarczająco dobre” do praktycznego zastosowania. Możesz zaimplementować algorytm, który dostosowuje żyroskop w zależności od niektórych mierzonych współczynników szumu, ale stałe wartości będą dobrze działać w większości zastosowań.

Jesteśmy o krok od uzyskania naszych zaktualizowanych szacunkowych wartości:

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

Teraz ponownie znormalizujmy ten wektor:

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

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

I jesteśmy gotowi do ponownego powtórzenia naszej pętli.

Ten przewodnik pierwotnie pojawił się na starlino.com, dokonałem kilku drobnych zmian i ponownie opublikowałem go za zgodą. Dzięki Starlino!