Obliczanie skoku, odchylenia i przechylenia na podstawie danych mag, acc i żyroskopów

19

Mam tablicę Arduino z czujnikiem 9 stopni swobody, z którego muszę określić skok, odchylenie i przechylenie deski.

Oto przykład jednego zestawu danych z czujnika 9-DOF:

Akcelerometr (m / s)

  • AccX = -5,85
  • AccY = 1,46
  • AccZ = 17,98

Żyroskop (RPM)

  • GyrX = 35,14
  • GyrY = -40,22
  • GyrZ = -9,86

Magnetometr (Gauss)

  • MagX = 0,18
  • MagY = -0,04
  • MagZ = -0,15

Jak obliczyć nachylenie, odchylenie i rzut na podstawie tych danych?

MaltDew
źródło
1
Podstawowa zasada: po wykryciu grawitacji w akcelerometrze wiesz, która droga jest w dół; po wykryciu ziemskiego pola magnetycznego w magnetometrze wiesz, którą drogą jest północ. Na tej podstawie i przy założeniu braku innych znaczących przyspieszeń lub silnych pól magnetycznych możesz określić własne nastawienie.
welf
1
Dane żyroskopu zapewniają szybkość obrotu, ale nie pozycję bezwzględną. Można go zintegrować w celu oszacowania zmiany w stosunku do znanej postawy, ale zazwyczaj jest to hałaśliwe i podatne na znoszenie, jeśli nie jest używane w połączeniu z innymi czujnikami.
welf
1
odnoszą się również do filtrów kalmana, ponieważ liczby statyczne muszą być dość często przetwarzane, aby dać wiarygodne szacunki pochylenia i odchylenia. Należy również pamiętać, że pozycja czujnika jest ważna (należy to wziąć pod uwagę).
Gürkan Çetin

Odpowiedzi:

15

Skok, przechylenie i odchylenie są definiowane jako obrót wokół osi X, Y i Z. Poniżej jako obrazek ilustrujący definicję.

Roll Pitch and Yaw

W poprzednim projekcie użyłem akcelerometru ADXL345 firmy Analog Devices do obliczenia przechyłu i nachylenia. Poniżej znajdują się równania użyte do obliczenia przechyłu i skoku. Udostępniłem część kodu źródłowego do użytku publicznego.

accelerationX = (signed int)(((signed int)rawData_X) * 3.9);
accelerationY = (signed int)(((signed int)rawData_Y) * 3.9);
accelerationZ = (signed int)(((signed int)rawData_Z) * 3.9);
pitch = 180 * atan (accelerationX/sqrt(accelerationY*accelerationY + accelerationZ*accelerationZ))/M_PI;
roll = 180 * atan (accelerationY/sqrt(accelerationX*accelerationX + accelerationZ*accelerationZ))/M_PI;

Pełny kod źródłowy można znaleźć tutaj .

Oprzyj się na powyższych definicjach

yaw = 180 * atan (accelerationZ/sqrt(accelerationX*accelerationX + accelerationZ*accelerationZ))/M_PI;

Uwaga: M_PI = 3,14159265358979323846 jest stała zdefiniowana w matematyce. H

Poniżej znajdują się odniesienia, w tym podstawowy kod źródłowy Arduino, które mogą ci pomóc.


Bibliografia:

Mahendra Gunawardena
źródło
2
Dobra odpowiedź, warto dodać, że położenie i orientacja czujnika w pojeździe byłyby ważne, a dane muszą być dalej przetwarzane, aby dać wiarygodne wyniki. (przefiltrowane lub połączone z bardziej wiarygodnymi danymi o niskiej częstotliwości, takimi jak GPS)
Gürkan Çetin
(@Zubair) „yaw = 180 * atan (przyspieszenie Z / sqrt (przyspieszenieXaccelerationX + przyspieszenieZaccelerationZ)) / M_PI;” co to jest „M_PI” ??
Wasabi
@Wasabi M_PI = 3,14159265358979323846. Jest stała zdefiniowana w bibliotece math.h.
Mahendra Gunawardena,
9

Tak więc moja dłuższa odpowiedź poniżej zakłada, że ​​deska ulegnie przyspieszeniu i w tym czasie nadal będziesz w stanie zmierzyć swój skok, przechylenie i odchylenie w krótkim czasie. Jeśli tablica będzie nieruchoma dla wszystkich pomiarów, odpowiedź Mahendry Gunawardena będzie dla ciebie idealna. Jeśli trafia to na urządzenie takie jak samolot segway lub model, multirotor lub cokolwiek, co się porusza, możesz chcieć czytać dalej. Ten post mówi o tym, jak używać wszystkich trzech czujników za pomocą metody zwanej łączeniem czujników. Połączenie czujników pozwala uzyskać mocne strony każdego czujnika i zminimalizować skutki słabości każdego czujnika.

Charakterystyka czujnika i tło

Najpierw zrozum, że akcelerometr mierzy wszystkie przyłożone do niego siły, a nie tylko siłę grawitacji. Tak więc w idealnym świecie z akcelerometrem w pozycji stacjonarnej bez wibracji można idealnie określić, która strona jest w górę, za pomocą podstawowej trygonometrii, jak pokazano w odpowiedzi Mahendry Gunawardeny. Ponieważ jednak akcelerometr wychwytuje wszystkie siły, wszelkie drgania powodują hałas. Należy również zauważyć, że jeśli tablica przyspiesza, nie można po prostu użyć prostej trygonometrii, ponieważ siła zgłaszana przez akcelerometr jest nie tylko ziemską siłą grawitacji, ale także siłą, która powoduje przyspieszenie.

Magnetometr jest prostszy niż akcelerometr. Ruch nie spowoduje problemów, ale takie rzeczy jak żelazo i inne magnesy ostatecznie wpłyną na twoją wydajność. Jeśli źródła powodujące te zakłócenia są stałe, nie trudno będzie sobie z nimi poradzić, ale jeśli źródła te nie są stałe, wytworzy mnóstwo szumów, które trudno jest usunąć.

Spośród trzech czujników żyroskop jest najbardziej wiarygodny i zwykle bardzo dobrze mierzą prędkość obrotową. Nie wpływają na to źródła żelaza, a przyspieszenia nie mają zasadniczo wpływu na ich zdolność do pomiaru prędkości obrotowej. Bardzo dobrze radzą sobie z raportowaniem prędkości, z jaką obraca się urządzenie, jednak ponieważ szukasz kąta bezwzględnego, musisz zintegrować prędkość, aby uzyskać pozycję. Spowoduje to dodanie błędu ostatniego pomiaru do błędu nowych pomiarów, ponieważ całkowanie jest w zasadzie sumą wartości w zakresie, nawet jeśli błąd dla jednego pomiaru wynosi tylko 0,01 stopnia na sekundę, w 100 pomiarach twoja pozycja można wyłączyć o 1 stopień, o 1000 pomiarów, można wyłączyć o 10 stopni. Jeśli wykonujesz setki pomiarów na sekundę, widać, że powoduje to problemy. Jest to powszechnie nazywane dryfem żyroskopowym.

Połączenie czujnika

Piękno współpracy wszystkich tych czujników polega na tym, że można użyć informacji z akcelerometru i magnetometru, aby anulować dryf żyroskopu. Dzięki temu możesz podać dokładność i szybkość żyroskopu bez fatalnej wady dryfowania żyroskopu.

Łącząc dane z tych trzech czujników można wykonać na więcej niż jeden sposób, powiem o użyciu filtra uzupełniającego, ponieważ jest on znacznie prostszy niż filtr Kalmana i filtry Kalmana pochłaniają znacznie więcej zasobów w systemach osadzonych. Często komplementarny filtr jest wystarczająco dobry, prostszy do wdrożenia (zakładając, że nie korzystasz z gotowej biblioteki) i pozwala szybciej przetwarzać dane.

Teraz na proces. Pierwszym krokiem, który musisz zrobić, jest zintegrowanie wyjścia żyroskopu w celu przekształcenia prędkości kątowej w pozycję kątową. Najprawdopodobniej będziesz musiał zastosować filtr dolnoprzepustowy na akcelerometrze i magnetometrze, aby poradzić sobie z hałasem na wyjściu. Działa tutaj prosty filtr FIR, taki jak pokazany poniżej. W przypadku niektórych trygonometrii można znaleźć pochylenie i kołysanie za pomocą akcelerometru i odchylenie za pomocą magnetometru.

filteredData = (1-weight)*filteredData + weight*newData

Ciężar jest tylko stałą, którą można regulować w zależności od tego, z jakim hałasem masz do czynienia, im wyższy poziom hałasu, tym mniejsza będzie jego wartość. Teraz łączenie danych z czujników można wykonać za pomocą następującego wiersza kodu.

fusedData = (1-weight)*gyroData + weight*accelMagData

Należy zauważyć, że dane są wektorem nachylenia, przechylenia i odchylenia. Możesz po prostu użyć do tego trzech zmiennych zamiast tablic, jeśli chcesz. Do tego obliczenia żyroskop podaje pozycję w stopniach nachylenia, przechyłu i odchylenia, magnetometr zapewnia kąt odchylenia, a akcelerometr podaje własne wartości pochylenia i przechylenia.

Jeśli nadal chcesz uzyskać więcej informacji, możesz znaleźć w Google „połączenie czujnika z filtrem uzupełniającym”, jest wiele artykułów na ten temat.

Dave851
źródło
3

Na podstawie danych z czujnika przyspieszenia można obliczyć tylko nachylenie i przechylenie. Poniższy dokument Freescale wyjaśnia dużą ilością potrzebnych informacji:

AN3461 - Wykrywanie przechyłu za pomocą trójosiowego akcelerometru

Na podstawie powiedzeń dokumentu

dębnikϕxyz=solpysolpz

dębnikθxyz=-solpxsolpygrzechϕ+solpzsałataϕ=-solpxsolpy2)+solpz2)

co równa się:

roll = atan2(accelerationY, accelerationZ)

pitch = atan2(-accelerationX, sqrt(accelerationY*accelerationY + accelerationZ*accelerationZ))

Oczywiście wynik jest taki tylko wtedy, gdy obroty występują w określonej kolejności (Rxyz):

  1. ϕ
  2. θ
  3. ψ

Rxyzψ

ekalyvio
źródło
1
Witaj w Engineering SE! Ta strona obsługuje lateks. Zobacz, jak piękna stała się Twoja odpowiedź. :-)
Peter - Przywróć Monikę