[stm32] - czujnik orientacji - żyroskop + akcelerometr

witam,

jestem w trakcie budowy robota półautonomicznego, który ma przeszukiwać zadaną wymiarami powierzchnią. chciałbym aby na podstawie danych z żyroskopu kompensował sobie odchylenie toru podczas jazdy do przodu, oraz podczas skrętu np. o 90 stopni wiedział, że na pewno skręcił o te 90 stopni.

próbowałem wykonać coś takiego za pomocą kompasu elektronicznego, ale miałem duże problemu z komunikacją po I2C. [ układ MMC2120MG].

zatem chciałbym wykorzystać żyroskop dwuosiowy X/Z z wyjściami analogowymi.

Mój problem to wymyślenie algorytmu, który w miare poprawnie sterowałby robotem. W jaki sposób wyłuskać z danych z żyroskopu kąt o jaki obrócił się robot np podczas jazdy na wprost, tak aby skorygować ten tor poprzez skręcenie kołami przednimi. Wiem, że aby z prędkości kątowej otrzymać drogę w stopniach trzeba scałkować sygnał. ale czy to ma sens i czy jest konieczne, czy może istnieje jakiś inny sposób?

żyroskop to: Moduł LPY5150AL - żyroskop analogowy . posiadam również moduł akcelerometru 3 osiowe z wyjściami analogowymi : KAmodMEMS2. czytałem że można te 2 czujniki połączyć ze sobą i stworzyć w jakiś sposób czujnik orientacji robota w płaszczyźnie poziomej. - pytanie tylko jak?


algorytm miałby być zaimplementowany na STM32.

Będę bardzo wdzięczny za jakąkolwiek pomoc. gdyż że jestem mechanikiem, elektronika czasem bywa czarną magią - a przetwarzanie sygnałów ... przetwarzaniem sygnałów

szkoda ze masz giroskop 2 osiowy. Aby określić położenie potrzebujesz 3... co do zakrętu. giroskopy się do tego NIE NADAJĄ. Trzeba je całkować, co powoduje nawarstwiający się błąd, do tego mają dryf, który skutecznie psuje nasz pomiar.
Co do reszty(pochylenie i przechylenie robota) wystarczą 2 żyra stabilizowane akcelerometrami. żyro, musisz tak ustawić, aby osie żyra były wzdłuż osi obrotu rabota. Do tego musisz co jakis czas likwidowac dryf żyroskopów akcelerometrami(funkcja atan2 z AX/AZ i AY/AZ)
co do obrotu... za pomocą magnetometru, to kiepski pomysl. za pomoca żyroskopu również... Ale oba te czujniki... to już co innego tak jak poprzednio, odczyt z żyroskopu + likwidacja dryfu magnetometrem. I śmiga . co do odczytów żyra, zapoznaj się z kątami Eulera, EWENTUALNIE kwaterniony, ale to już troszke wyższa szkoła jazdy. Ponieważ twój robot nie lata, nie potrzebujesz BARDZO zaawansowanych obliczeń, jeśli kąty eulera to za dużo, odczyt z żyra i likwidacja dryfu akcelerometrem wystarczy. Robot musiałby się przechylać, odchylać i pochylac naraz, aby odczyt z zyroskopu stał się bezużyteczny. Jak masz więcej pytań... pisz.

Zrobiłem wyznaczenie orientacji na podstawie pomiarów z akcelelometrów i żyroskopów używając algorytmu - filtru "Efficient Orientation Filter for inertial sesnors" stworzonego przez dr Sebastiana Madgwick’a. Link:
http://imumargalgorithm30042010sohm.googlecod...0and%20inertialmagnetic%20sensor%20arrays.pdf

Zastosowanie i działanie algorytmu można zobaczyć tu: https://www.elektroda.pl/rtvforum/topic1997601.html#9518334

ale w jaki sposób połączyć dane z kompasu i żyroskopu? gdzieś wyczytałem że należy je przepuścić przez filtr kalmana? kiedyś tego używałem na jakiejś laborce ale i tak to czary mary.

jeżeli mam jakieś dane z gyro ( napięcie jest próbkowane z jakąś częstotliwością) i dane z magnetometru ( po dane z I2C jeżeli uda się uruchomić) to oba sygnały filtrujemy filtrem kalmana? i mamy jakieś wartości to co dalej?

rozumiem że magnetometr miałby być aby likwidować dryft zera żyroskopu? a czy nie można by po prostu co jakiś czas kalibrować żyra? niech robot sopbie staje w miejscu na jakiś czas i żyro niech sie skalibruje? było by to możliwe aby jak najbardziej uprościć całość?


z góry dzięki za odpowiedź.

oj panie! zostaw filtr kalmana. Mówię serio. Napisałem BARDZO rozbudowany filtr kalmana do łączenia danych z giroskopu i akceleromteru... a duzo lepiej się spisywał najprostrzy w świecie filtr... sprawa jest prosta. jak odczytać kąt z żyra? żyro podaje prędkość kątową. trzeba to całkować. metod bez liku, ale jak bedziesz mial czas to polecam całkowanie z interpolacją wielomianową. jeśli nie masz czasu... metoda prostokątów wystarczy . jesli masz żyro analogowe to napięcie odczytywane przez nie to prędkość kątowa. wystarczy jedynie znaleźć w dokumentacji żyra przelicznik napięcie->prędkość. Jak mamy prędkość... całkujemy. Mamy kąt. tylko problem jest w tym, że żyro dryfuje + błąd całkowania. eliminujemy go przez akcelerometry. obliczamy kąt z akcelerometru(atan2(ax,az) oraz atan2(ay,az)) mamy w ten sposób pochylenie i przechylenie. nastepnie obliczmy blad miedzy odczytem kąta z żyra a kątem z akcelerometrów(kąt - kąt_acc). wzmacniamy błąd wartością mniejszą od 1 i odejmujemy przy całkowaniu(przykład: kąt += (prędkość_kątowa*dt - obliczony_błąd*wzmocnienie)). wartość wzmocnienia na początek najlepiej ok 0,05. im większa wartość tym odczyt bardziej "drga" im mniejsza wartość tym odczyt ma większą tendencję do powolnej likwidacji dryfu(na przykład przy szybkich ruchach). Musisz dobrać doświadczajnie. Jak juz bedziesz mial, można eksperymentować ze zmiennynm wzmocnieniem błędu. Kąty eulera obliczysz z "Układy sterowania automatycznego samolotem" pana Stanisława Boćka. filtr prymitywny, ale skuteczny

Dodano po 2 [minuty]:

co do magnetometru, zagadnienie traktujemy jak akcelerometry z tą różnicą że liczymy atan2(mx,my). Działa to niestety tylko dla względnie malych kątów przechylenia i pochylenia. generalnie powinienes zastosować macierz obrotu o te kąty to "wyrównania" składowych wektora magnetycznego.

Podłączam się do tematu ponieważ w raz ze znajomymi będę budował segway, ale nie tego z http://www.zzaag.org/index_eng.html, ma to być konstrukcja bardziej samodzielna (okaże się zresztą).
Czy dane z akcelerometru przed porównaniem z kątem od żyra przepuszczamy przez jakiś filtr cyfrowy np. NOI czy jest to raczej nie dopuszczalne z racji wprowadzania pewnych opóźnień?

morurd napisał:

tylko problem jest w tym, że żyro dryfuje + błąd całkowania. eliminujemy go przez akcelerometry. obliczamy kąt z akcelerometru

A co robisz z dryftem akcelerometrów w funkcji temperatury? Termostatyzujesz je? Co z kompensacją dużego błędu żyroskopu w chwili gdy akcelerometr mierzy dodatkową składową przyspieszenia wynikającą np. z hamowania?

pozdrawiam

a widzisz. to nie problem. co do temperatury to nie zaobserwowałem znacznego jej wpływu na odczyty kąta. oczywiście, w wolnej chwili przysiąde, ale jak narazie nie ma takiej potrzeby(prawdopodobnie trzeba będzie zamrozić czujnik w zamrażalniku i zrobić wyres a(temp.) wtedy będzie charakterystyka, akcelerometry 3-osiowe czesto mają termometry, mój ma). co do manewrów z dużym przyspieszeniem... można caly czas usredniac wartość przyspieszenia. w momencie wykrycia większej wartości przyspieszenia(limit można dobrać doświadczalnie), niz do tej pory uśredniona, mnozymy wzmocnienie błędu przez jakąś małą wartość. mechanizm sprawdzony, działa dobrze.

Dodano po 2 [minuty]:

DosinskY napisał:

Co z kompensacją dużego błędu żyroskopu w chwili gdy akcelerometr mierzy dodatkową składową przyspieszenia wynikającą np. z hamowania?

pozdrawiam

chodziło o akcelerometr?

Kiedyś dość intensywnie zajmowałem się przyspieszeniomierzem ADXL210 - całkiem mocno pływał przy zmianach temperatury. W owych czasach był to jeden z na prawdę niewielu tego typu czujników na rynku. Teraz można powiedzieć, że mamy do czynienia z wysypem prawdopodobnie parametry też uległy poprawie.

Chodziło mi o niwelowanie błędu kąta obrotu uzyskanego na drodze całkowania prędkości kątowych z żyroskopu wartościami na podstawie odczytów akcelerometrów (przy istotniejszych przyspieszeniach). Odpowiedź w zasadzie już podałeś. Musze kiedyś zbudować sobie takie coś i pobadać.

Pozdrawiam

Znalazłem w sieci taki schemat blokowy:


filtr zastosowany ma postać:
angle = A * (angle + rate * DT) + (1 - A) * (float) accel_raw * A_GAIN;

link do pełnego opisu: Tutaj
Czujnik przeznaczony do zastosowania w płaszczyźnie pionowej.

Jeżeli użyję kompasu zamiast akcelerometru to będzie się nadawać równie dobrze do płaszczyzny poziomej?
Czy ten filtr jest taki sam zarówno dla żyroskopu jak i akcelerometru/komasu tylko z różnym współczynnikiem A? - nie do końca zrozumiałem opis.

niegłupie... naprawdę niegłupie... ale tym A i tak musisz dynamicznie sterować(dla duzych przyspieszeń A musi rosnąć).
mozesz go oczywiście użyć do magnetometru, A może być wtedy bliskie 1.. tak myslę, ale to kwestia eksperymentowania.
Teraz tylko się zastanawiam po co A_GAIN...? zamiana na rad->deg? pewnie tak... ale myślę że to oczywiste

Zmienna A_Gain służy do przeliczenia radianów na stopnie
Jest to jeden z podstawowych algorytmów fuzji pomiarów, w internecie znajduje się bardzo dużo informacji w języku angielskim - "Complementary Filter". Na tym etapie nie warto samemu wymyślac algorytmów fuzji pomiarów, należy zapoznać się z aktualnym stanem wiedzy, zrozumieć i dopiero później próbowac tworzyc coś lepszego. Umieszczę kilka dokumentów od których zaczynałem naukę.

Podstawy oraz prosta implementacja algorytmu http://www.starlino.com/imu_guide.html

Opisane w prosty sposób zagadnienia związane z INS: "Inertial sensors K.F Complex Filter.pdf"

Słynne opracowanie filtru złożonego przez Roberta Mahony'a "2008_MahHamPfl.TAC"

Osobiście polecam reazlizowac wyznaczanie orientacji wg algorytmu Sebastian'a O.H. Madgwick'a.

Są to mniej złożone obliczeniowo algorytmy, o mniejszych wymaganiach co do mocy obliczeniowej niż filtr Kalmana. Bardziej złożonym algorytmem, który nadaje się do profesjonalnych zastosowań jest Extended Kalman Filter.

ok, przejrzałem algorytm pana Madgwick'a. wydaje mi się, że przewiduje on, że żyroskopy znajdują się w układzie związanym z pierwszym położeniem, a nie z układem lokalnym(czujnika). Jak liczyles połozenie(same giroskopy)? skorzystałeś z zamieszczonych programów na końcu? staram się tego uniknąć... uniemożliwi pełne zrozumienie tematu

Mam pytanie odnośnie poniższego równania na wyznaczanie kąta:

jacekhladysz napisał:

angle = A * (angle + rate * DT) + (1 - A) * (float) accel_raw * A_GAIN

W opisie jest napisane:
accel_raw*A_GAIN is the accelerometer reading, converted to degrees. It is very important that these two be in the same unit base before adding together.

Docelowo chcę mierzyć osobno pochylenie w osi X i Y z użyciem akcelerometru 3-osiowego i żyroskopu 2-osiowego. Jakiego wzoru mam użyć aby z odczytu z akcelerometru uzyskać kąty pochylenia dla poszczególnych osi? Brałem pod uwagę ten opis:



Czy zgodnie z tym opisem, powinienem użyć funkcji arcus cosinus? Z góry dziękuję za odpowiedź.

jeżeli liczysz pochylenie i przechylenie wystarczy użyć genialnej funkcji udostępnianej za pośrednictwem nagłówka "math.h" mianowicie atan2 (lub atan2f). Następnie mamy atan2(Ax,Az) oraz atan2(Ay,Az). Funkcja zwraca kąty w radianach. Nic więcej naprawdę nie potrzeba. Jeśli Cię to interesuje to w internecie dowiesz się jak dokładnie działa atan2.

Dziękuję bardzo za odpowiedź. Czytałem o użyciu tej funkcji i zastanawiałem się jak to się ma do opisu, który wrzuciłem powyżej. Rozumiem więc, że tak obliczony kąt w radianach przeliczam na stopnie i mogę go z powodzeniem umieścić w tym wzorze jako składnik "accel_raw * A_GAIN"?

jacekhladysz napisał:

angle = A * (angle + rate * DT) + (1 - A) * (float) accel_raw * A_GAIN

Mam jeszcze pytanie odnośnie pogrubionego elementu angle. Nie jest to opisane, ale oczywiście chodzi wartość kąta z poprzedniego przejścia pętli?

tak jest. Zapis angle = angle + rate*DT to nic innego jak całkowanie metodą prostokątów. Osobiście polecam metodę trapezów, niewiele więcej obliczeń a sporo większa dokładność