Estymacja prędkości na podstawie danych z akcelerometru.

[EasyPeasy_]

Wyskalowałem sobie dane przy odczycie z akcelerometru do m/s^2 następnie zebrałem 100 danych (co 10 ms 100 próbek = 1 sekunda zbierania), w których poruszyłem IMU tylko w osi pionowej Z, a konkretniej zrobiłem szybki ruch do góry i zatrzymałem się na górze.

To te dane: http://wklej.org/id/3288545/

Przechodząc do meritum, mianowicie jak na ich podstawie wyestymować prędkość w poszczególnej chwili czasowej?

______________________

Ustalmy co wiem lub co się dowiedziałem, przyśpieszenie to pochodna prędkości, więc teoretycznie, żeby otrzymać prędkość należy scałkować przyśpieszenie. Teraz nie wiem czy powinienem całkować bezpośrednie odczyty przyśpieszenia z akcelerometru czy tylko różnice pomiędzy kolejnymi odczytami?

Sprawdziłem obie opcje (nie wiem czy którakolwiek jest dobra) i dla pierwszej prędkość rosła w nieskończoność dla drugiej, którą zrealizowałem takim algorytmem:

Vz = Vz + (odczyt_akcelerometr_w_osi_z-poprzedni_odczyt) * krok_czasu;
poprzedni_odczyt=odczyt_akcelerometr_w_osi_z;

I w zasadzie to nie wiem czy dobrze czy źle. Jak robiłem próby i poruszałem IMU szybko w górę to najwyższy odczyt jaki otrzymałem to był 0.8, a więc skoro całkuję przyśpieszenie w m/s^2, więc wydaje mi się, że prędkość otrzymuje w m/s, a więc te 0.8 m/s to 2.88 km/h (tak wolno poruszam ręką ?).

Co więcej wziąłem laptopa, arduino i IMU do samochodu. Stanąłem odpaliłem pomiar i dałem gaz w podłogę, i odczyty prędkości nie odpowiadały prędkości podczas przyśpieszania do 60km/h, bo nie przekroczyły 1.0, a 60 km/h to 16.66 m/s, więc nie wiem czy ten algorytm jest dobry?

I powiedźcie mi po co w czujniku IMU jest możliwość skalowania wyników np. akcelerometr ma opcje +2/4/8/16g a żyroskop 250/500/1000/2000DPS ja przy pomiarach miałem ustawione na +2g i 250DPS.

Może dlatego akcelerometr przy próbie z samochodem nie dał mi takich odczytów jakich oczekiwałem?

[marek1707]

1. Żeby dostać prędkość musisz całkować przyspieszenia a nie żadne tam różnice.

2. Akcelerometr nie odróżnia grawitacji od przyspieszenia "bezwzględnego" więc czujnik "zanurzony" w polu grawitacyjnym Ziemi będzie pokazywał stały wektor 9.81m/s² w dół. Jeśli czytałeś oś Z a płytka leżała dokładnie poziomo, dostawałeś w niej stałe odczyty właśnie tej wartości. Po scałkowaniu jedziesz w stronę nieskończoności, to jasne. Musisz pomiary najpierw skalibrować czytając je w spoczynku a potem uwzględniać ten offset (z odpowiednim znakiem) w obliczeniach.

3. Przyspieszenia czytasz z akcelerometru w jakichś jednostkach. Przestawianie zakresów zmienia te jednostki. Im wyższy zakres tym więcej możesz mierzyć, ale za to spada rozdzielczość. Nie wiem co tam masz za czujnik, ale przykładowo mając np. rejestr 16-bitowy z maksymalnym odczytem 65535 i zakres 2g (czyli 19.62m/s²), kwant odczytu wyniesie 0.3mm/s² i przez tyle musisz skalować odczytane liczby by dostać wynik w m/s². Takie rzeczy są zawsze opisane w datasheet scalaka. Wczytaj się w opisy rejestrów i w sposób przeliczania odczytów na wielkości fizyczne. Pamiętaj, że wszelkie czujniki szumią i nie od parady mają wbudowane różne filtry. Używaj ich. Aby całkować jak najlepiej musisz mierzyć jak najczęściej, ale im częściej mierzysz tym filtrowanie jest gorsze i szumy rosną. Tu właśnie zaczynają się różnice między dobrymi i drogimi czujnikami a słabymi i tanimi. Zbyt rzadkie pomiary powodują, że Twoje całkowanie pomija niuanse zmian, bez uwzględniania których rośnie błąd w czasie. Prawie na pewno po ruszeniu, jeździe i zatrzymaniu pojazdu nie dostaniesz v=0 i tu właśnie wkraczają fuzje różnych czujników bo np. enkodery z kół lub silników "wiedzą", że na pewno stoisz i mogą zerować narastające uchyby całkowania. Musisz się nad tym porządnie zastanowić.

EDIT: Jeśli pełna skala wynosi ±2g a nie po po prostu od 0 do 2g, to oczywiście kwant pomiaru przy 16 bitach wynosi 0.6mm/s² a odczytywane są wartości ze znakiem.

EDIT2: Mam nadzieję, że to rozumiesz, ale tylko dla przypomnienia: całkowanie numeryczne tylko wygląda jak zwykłe dodawanie kolejnych wartości (ze znakiem). Tak naprawdę jest sumowaniem pól powierzchni pod kolejnymi prostokątami - w najprostszym przypadku. A jeśli pól, to musisz uwzględniać nie tylko wysokość ale i szerokość prostokątów czyli tutaj - odstęp czasu. Fizycznie oznacza to, że jeśli robisz odczyty powiedzmy co 20ms i za każdym razem odczytujesz przyspieszenie 1m/s² to z każdym odczytem prędkość nie rośnie o 1m/s tylko o 0.02m/s, bo czas był tyle razy krótszy.

[EasyPeasy_]

2. Akcelerometr nie odróżnia grawitacji od przyspieszenia "bezwzględnego" więc czujnik "zanurzony" w polu grawitacyjnym Ziemi będzie pokazywał stały wektor 9.81m/s² w dół. Jeśli czytałeś oś Z a płytka leżała dokładnie poziomo, dostawałeś w niej stałe odczyty właśnie tej wartości. Po scałkowaniu jedziesz w stronę nieskończoności, to jasne. Musisz pomiary najpierw skalibrować czytając je w spoczynku a potem uwzględniać ten offset (z odpowiednim znakiem) w obliczeniach.

Jedno pytanie jak skalibrować, bo ja całkowałem właśnie ten odczyt z osi Z i napisałem już Panu, że przeliczyłem jednostki tak, żeby odczytywać wartości w m/s^2.

Czy zgodnie z tym dokumentem powinienem od wartości odczytywanych z akcelerometru odjąć składową wartości grawitacji w danej osi przez ten wzór: http://pe.org.pl/articles/2014/1/9.pdf

a następnie całkować przyśpieszenia zgodnie z tymi algorytmami (dla osi Z jest przedstawiony w tym dokumencie -> http://www.kms.polsl.pl/mi/pelne_13/15_13_44.pdf, dla innych osi na podstawie odejmowania składowania grawitacji w danej osi wymyśliłem swoje algorytmy dla osi x oraz y) żeby odczytywać wartości prędkości w danej osi:

Gdzie poszczególne kąty to:

[marek1707]

1. Kalibrujesz swoje IMU w spoczynku, najlepiej we wszystkich trzech osiach. Polega to na "zdjęciu" długości wektora grawitacji dla wielu kierunków. Obracasz, mierzysz, obracasz, mierzysz. W rezultacie dostajesz jakąś powierzchnię, zwykle jest to jakieś jajko (bo nigdy trzy osie/czujniki nie będą identyczne) po której poruszał się koniec wektora grawitacji. Przez interpolację możesz tę powierzchnię odtworzyć lub założyć, że jest to idealna kula. Od tej pory wiesz co pokazuje Twój czujnik w dowolnym położeniu gdy nie widzi żadnego przyspieszenia.

2. Liczysz moduł (długość) i kierunek aktualnego wektora wypadkowego przyspieszenia na podstawie odczytu wszystkich trzech osi.

3. Znając długość wektora grawitacji w tym kierunku (z kalibracji), odejmujesz ich długości i to co zostaje to "dodatkowe" przyspieszenie którego szukasz. W tym kierunku i z takim gradientem rośnie prędkość. Nie musi być ona styczna do płaszczyzny podwozia właśnie dlatego, że trasa ma górki/dołki/dywany itp. A nawet jeśli jest, to wcale nie znaczy, że platforma jest ustawiona poziomo i prędkość rzeczywista wcale nie musi przekładać się na prędkość na płaszczyźnie mapy. Musisz znać pochylenia z żyroskopów i (skalibrowanego) magnetometru 3D lub (dla uproszczenia) założyć, że podłoga jest zawsze płaska i pozioma. Złą wiadomością jest, że "to co zostaje" jest małe i tak zaszumione, że praktycznie nie do wykorzystania.

Na 100% nie da się tu wymyślić niczego lepszego niż to co zostało już zrobione a rozwiązania wymyślone na poczekaniu w zasadzie są bezużyteczne:

http://www.chrobotics.com/library/accel-position-velocity

https://www.novatel.com/assets/Documents/Bulletins/APN064.pdf

Jeśli nie masz czasu na napędy, na to także nie masz tym bardziej. Użyj gotowych bibliotek np. filtrów komplementarnych lub kodów z FC łazików lub quadrocopterów:

http://ardupilot.org/rover/

https://learn.adafruit.com/adafruit-9-dof-imu-breakout/software

http://wiki.paparazziuav.org/wiki/Main_Page

https://github.com/PX4/Firmware/tree/master/src/modules

[EasyPeasy_]

1. Kalibrujesz swoje IMU w spoczynku, najlepiej we wszystkich trzech osiach. Polega to na "zdjęciu" długości wektora grawitacji dla wielu kierunków. Obracasz, mierzysz, obracasz, mierzysz. W rezultacie dostajesz jakąś powierzchnię, zwykle jest to jakieś jajko (bo nigdy trzy osie/czujniki nie będą identyczne) po której poruszał się koniec wektora grawitacji. Przez interpolację możesz tę powierzchnię odtworzyć lub założyć, że jest to idealna kula. Od tej pory wiesz co pokazuje Twój czujnik w dowolnym położeniu gdy nie widzi żadnego przyspieszenia.

2. Liczysz moduł (długość) i kierunek aktualnego wektora wypadkowego przyspieszenia na podstawie odczytu wszystkich trzech osi.

No szczerze mówiąc mimo wielkich moich chęci To jak będzie mi Pan to tak opisywał to nic z tego nie wyjdzie, ja potrzebuję informacji zrób to i tak, w ten sposób, takim algorytmem

Podałem Panu sposoby z dokumentów, które znalazłem w celu odjęcia grawitacji są dobre czy nie? Ja otrzymuję tym sposobem właśnie zerowe przyśpieszenia, więc wydawać by się mogło, że w każdej z osi wektor grawitacji jest dobrze odejmowany. I tak samo z całkowaniem tego przyśpieszenia gdy czujnik stoi dostaje 0 prędkości w osiach równoległych do płaszczyzn, a w osi pionowej przyśpieszenie nie jest zerowe tylko powiedzmy 0.01 i przez to całkowanie prędkości w tej osi rośnie.

I nie wiem czy ten sposób, którym to zrealizowałem jest dobry czy nie. To właściwie cały kod, który to realizuje:

 
/*Pobranie danych z akcelerometru i żyroskopu*/
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
/*Przeskalowanie ich do m/s^2 dla skal +/-2 g 250 DPS*/
 aaax=ax*rangePerDigit * 9.80665f;
 aaay=ay*rangePerDigit * 9.80665f;
 aaaz=az*rangePerDigit * 9.80665f;
/* Obliczenie kąta Pitch i Roll*/
 accPitch = -(atan2(aaax, sqrt(aaay*aaay + aaaz*aaaz))*180.0)/M_PI;
 accRoll  = (atan2(aaay, aaaz)*180.0)/M_PI;
/* Obliczenie wektora grawitacji w poszczególnych osiach*/ 
 g_x=9.80665f*(-sin(accPitch*0.01745329f));
 g_y=9.80665f*(sin(accRoll*0.01745329f)*cos(accPitch*0.01745329f));
 g_z=9.80665f*(cos(accRoll*0.01745329f)*cos(accPitch*0.01745329f));
/*Obliczenie faktycznego przyśpieszenia IMU po odjęciu wektora grawitacji*/
 realax=aaax-g_x;
 realay=aaay-g_y;
 realaz=aaaz-g_z;
/* Całkowanie prędkości w poszczególnych osiach*/ 
 v_x=v_x+realax*0.01;
 v_y=v_y+realay*0.01;
 v_z=v_z+realaz*0.01;
/*Obliczenie prędkości w płaszczyźnie ruchu pojazdu (mój IMU jest tak skierowany, że oś Z jest w tej płaszczyźnie*/ 
v_h=sqrt(v_x*v_x + v_z*v_z);

3. Znając długość wektora grawitacji w tym kierunku (z kalibracji), odejmujesz ich długości i to co zostaje to "dodatkowe" przyspieszenie którego szukasz. W tym kierunku i z takim gradientem rośnie prędkość. Nie musi być ona styczna do płaszczyzny podwozia właśnie dlatego, że trasa ma górki/dołki/dywany itp. A nawet jeśli jest, to wcale nie znaczy, że platforma jest ustawiona poziomo i prędkość rzeczywista wcale nie musi przekładać się na prędkość na płaszczyźnie mapy. Musisz znać pochylenia z żyroskopów i (skalibrowanego) magnetometru 3D lub (dla uproszczenia) założyć, że podłoga jest zawsze płaska i pozioma. Złą wiadomością jest, że "to co zostaje" jest małe i tak zaszumione, że praktycznie nie do wykorzystania.

Na 100% nie da się tu wymyślić niczego lepszego niż to co zostało już zrobione a rozwiązania wymyślone na poczekaniu w zasadzie są bezużyteczne:

http://www.chrobotics.com/library/accel-position-velocity

https://www.novatel.com/assets/Documents/Bulletins/APN064.pdf

Jeśli nie masz czasu na napędy, na to także nie masz tym bardziej. Użyj gotowych bibliotek np. filtrów komplementarnych lub kodów z FC łazików lub quadrocopterów:

http://ardupilot.org/rover/

https://learn.adafruit.com/adafruit-9-dof-imu-breakout/software

http://wiki.paparazziuav.org/wiki/Main_Page

https://github.com/PX4/Firmware/tree/master/src/modules

No ok dziękuję za dokumenty na pewno po zajęciach dzisiejszych do nich zajrzę jak i za te biblioteki też je przejrzę