Kanał - ATNEL tech-forum
Wszystkie działy
Najnowsze wątki



Teraz jest 28 mar 2024, o 22:07


Strefa czasowa: UTC + 1





Utwórz nowy wątek Odpowiedz w wątku  [ Posty: 14 ] 
Autor Wiadomość
PostNapisane: 7 maja 2018, o 23:10 
Offline
Nowy

Dołączył(a): 17 sty 2013
Posty: 10
Pomógł: 0

Witam,
Wykonuję obsługę odczytu danych z żyroskopu L3GD20H. Dane są wysyłane z uC na komputer za pośrednictwem BTM-222. Transmisja przebiega prawidłowo. Na wejściu dostaję dane w postaci:
-ilość taktów zegara od ostatniego wystawienia nowych danych przez gyro dla F_CPU=8000 000 i preskalera 8 i dla ODR=200 dostaję TCNT1L = 125, TCNT1H=20. Po przeliczeniach wychodzi 190,66 [Hz] więc jest to wartość prawidłowa.
-dane pobrane z gyro mianowicie OUT_X_L, OUT_X_H, OUT_Y_L, OUT_Y_H, OUT_Z_L, OUT_Z_H.
Odbierane dane przy nieruchomym gyro są przetwarzane w procedurze inicjalizacyjnej w następujący sposób:
-pominięcie pięciu pierwszych odczytów - wartości znacznie odbiegające od pozostałych,
-wyznaczenie błędu spoczynkowego - wyliczenie średniej z kolejnych np 200 odczytów danych dla poszczególnych osi,
-wyznaczenie odchylenia standardowego i 3-sigma które jest nam później potrzebne aby rozpoznawać czy nastąpił już obrót czy jeszcze nie,
Po tym wstępie kolejne dane są już traktowane jako dane prawidłowe do wykonania odczytu. Operacje które na nich zachodzą to:
-wyliczenie wartości 16-bitowej odczytanej danej prędkości obrotu,
-obliczenie różnicy pomiędzy wartością zmierzoną a wartością błędu spoczynkowego i porównanie czy nie przekracza wartości 3-sigma, jeśli nie przekracza to ruch nie miał miejsca, jeśli przekracza to wartość bierzemy do dalszych operacji,
-pomnożenie wartości prędkości obrotu przez czułość,
-podzielenie prędkości obrotowej przez czas w którym zachodziła - w wyniku tego mamy kąt obrotu,
-wyznaczenie za pomocą kwaternionów pojedynczej osi obrotu, która zastępuje nam trzy składowe osie, punkt który obracamy wokół osi ma współrzędne początkowe P=(0,0,1) tak więc możemy wyznaczyć nowe położenie punktu P
-obliczenie kąta pomiędzy osią Z=(0,0,1) a nowym położeniem P'

Potrzebuję mierzyć kąt jaki występuje pomiędzy osią pionową skierowaną prostopadle do powierzchni ziemi a osią Z gyro. Ta oś jest równoznaczna z pierwotnym położeniem osi Z gyro. Tak więc jeśli obrócę gyro i dowolne kąty i następnie powrócę do pozycji początkowej to kąt wychyłu powinien wrócić do zera. Wykonuję obliczenie kąta jaki występuje pomiędzy osią (wektorem) globalną Z a osią (wektorem) Z gyro. Położenie końca osi Z gyro jest wyznaczane przez położenie punktu P którego początkowe położenie to (0, 0, 1). Położenie punktu P zmienia się przy każdym obrocie gyro i jest wyznaczane za pomocą kwaternionu.

I tutaj zaczynają się problemy. Odchylam gyro od osi globalnej która jest zorientowana pionowo do powierzchni ziemi (gyro też w położeniu początkowym ma również oś Z zorientowaną pionowo). Jeśli odchylam gyro wokół osi X o 90 stopni i wracam do położenia pierwotnego to wskazanie kąta odchylenia również zmienia się od 0 do 90 i ponownie na 0. Jednak jeśli przechył gyro wykonam względem osi nierównoległej do osi X i Y i następnie powrót to wskazanie nie wraca do zera a np do 30 stopni.

Nie mam już pomysłu gdzie robię błąd. Kwaterniony zastępują nam obroty wokół poszczególnych osi, które trzeba było by rozważać jako najpierw obrót np wokół X, potem Y a na końcu Z, ale jeśli byśmy zmienili na np Y, X i Z to położenie końcowe punktu P było by inne.
Liczba (wartość czasu) przez którą dzielimy prędkość kontową jest również pobierana z gyro więc nie ma tam błędu. Bezruch gyro jest również wykrywany.


Proszę o sugestię gdzie może tkwić błąd w moich rozważaniach. Nie chcę (na razie) stosować żadnych dodatkowych czujników np akcelerometru bo tok rozumowania i działań które przedstawiłem/zaimplementowałem jest sensowny ale przy multi-obrocie nie działa. Na pewno ktoś z tu obecnych również miał takie problemy.

Kod wyznaczania kwaternionu
https://en.wikipedia.org/wiki/Conversio ... ler_angles
Sposób wyznaczania błędu spoczynkowego od str 28
https://www.elecrow.com/download/TA0343.pdf
Kąt pomiędzy wektorami
http://eszkola.pl/matematyka/kat-miedzy ... -5489.html



Góra
 Zobacz profil  
cytowanie selektywne  Cytuj  
PostNapisane: 8 maja 2018, o 21:44 
Offline
Użytkownik

Dołączył(a): 05 wrz 2017
Posty: 169
Pomógł: 31

Widzę, że kolega zainteresował się odwrotnym zadaniem kinematyki - ciekawy temat.
Odnośnie samego problemu to proponuje po kolei eliminować możliwe błędy tj.

1. Przesyłać na PC "czysty" odczyt z żyroskopu, przez co rozumiem nie przeliczone w żaden sposób dane po prostu to co MEMS wysłał do mikrokontrolera. Pozwoli to określić czy dysponujesz wadliwym układem, czy gdzieś jest babol w kodzie. Jeśli wszystkie osie będą wracały do zera to odpowiedź będzie jasna.

2. Tak duży błąd o którym piszesz nie klasyfikuje się jako dryft z którym też powinieneś się zmierzyć, jestem zdania że tu filtr Kalmana będzie niezbędny. Określenie błędu spoczynkowego (błędu zera) raczej nie wystarczy, dryft można mocno ograniczyć filtrem górnoprzepustowym wbudowanym w układ ale z moich doświadczeń wynika, że to tylko ograniczenie i niestety zależne od egzemplarza (testowałem LSM9DS0, MPU6050 i jakiś chiński analogowy).

--- Sekcja wróżenia z fusów ---
Bez kodu źródłowego obstawiał bym, że "wadliwa" oś OZ to po prostu pomijanie odczytu/niepoprawna konfiguracja (różne czułości ?). Zwróć uwagę, że masz dość dużo obliczeń, przypuszczam że również zmiennoprzecinkowych. Co by było gdybyś pobierał dane dla osi OX i OY z chwili t0, a dla osi OZ z chwili t1 (tu oczywiście kwestia w jakim trybie masz bufor ustawiony)? W LSM9DS0 bardzo użyteczne były wyprowadzenia DRDY (data ready), zawsze miałem pewność, że dane już są, szkoda tylko 1 wyprowadzenia.

Pozdrawiam



Góra
 Zobacz profil  
cytowanie selektywne  Cytuj  
PostNapisane: 9 maja 2018, o 12:53 
Offline
Użytkownik

Dołączył(a): 28 wrz 2016
Posty: 215
Pomógł: 14

abel11 napisał(a):
Przesyłać na PC "czysty" odczyt z żyroskopu ...

bardzo dbry pomysł, zwłaszcza,że zamiast czytać dane z żyroskopu można czytać dane z testowego pliku
abel11 napisał(a):
... że tu filtr Kalmana będzie niezbędny

nie wydaje się by aż taki filtr byłby konieczny (choć nie zaszkodzi go zastosować), bo nie ma podstaw przypuszczać niestacjonarności procesu.

Ja spróbowałbym przetestować układ przy wykorzystaniu Arduino i dostępne biblioteki do żyroskopu. pozwoli namierzyć i wyeliminować błędy sprzętu.

Zgadując: postawiłbym na zbyt wysoki poziom "odcięcia", tzn zbyt wysoką wartość poniżej której nie rejestruje sie obrotu

_________________
de gustibus non est disputandum



Góra
 Zobacz profil  
cytowanie selektywne  Cytuj  
PostNapisane: 9 maja 2018, o 15:17 
Offline
Użytkownik

Dołączył(a): 05 wrz 2017
Posty: 169
Pomógł: 31

Każdy żyroskop charakteryzuje się dryftem, właśnie w celu wyeliminowania tego problemu stosuje się zintegrowane układy 6D i 9D. Filtr Kalmana wymaga 2 niezależnych źródeł sygnału (z uwagi na konstrukcję zintegrowanych MEMS ta niezależność jest umowna ), dlatego zwykle stosuje się żyroskop i akcelerometr. Czytałem też opracowania gdzie jako jeden z czujników stosowano, kompas lub nawet kamerę która określała położenie linii horyzontu (jeśli dobrze pamiętam Instytut Lotnictwa prowadził takie badania).

Oczywiście nie twierdzę, że to jedyne słuszne rozwiązanie problemu dryftu, aczkolwiek chyba nie bez powodu jest ono bardzo powszechne. Pozwala na pominięcie dobierania częstotliwości granicznych filtra górno i dolno przepustowego w celu ograniczenia szumu i dryftu, nie sprawdzałem które z rozwiązań jest finalnie bardziej złożone obliczeniowo (duży wpływ będzie miała stromość charakterystyki filtra FIR/IIR i czas propagacji sygnału). Może kiedyś pokuszę się o jakiś eksperyment w tym kierunku.



Góra
 Zobacz profil  
cytowanie selektywne  Cytuj  
PostNapisane: 9 maja 2018, o 21:29 
Offline
Nowy

Dołączył(a): 17 sty 2013
Posty: 10
Pomógł: 0

Odpisuję na niektóre z uwag/porad i podsyłam plik z pobranymi danymi w postaci:
TCNT1L TCNT1H
OUT_X_L OUT_X_H OUT_Y_L OUT_Y_H OUT_Z_L OUT_Z_H
TCNT1L TCNT1H
OUT_X_L OUT_X_H OUT_Y_L OUT_Y_H OUT_Z_L OUT_Z_H
TCNT1L TCNT1H
OUT_X_L OUT_X_H OUT_Y_L OUT_Y_H OUT_Z_L OUT_Z_H
...


Jako pierwsze sprawdziłem
Cytuj:
Zgadując: postawiłbym na zbyt wysoki poziom "odcięcia", tzn zbyt wysoką wartość poniżej której nie rejestruje sie obrotu

To raczej nie wchodzi w grę. Za każdym razem odchylenie standardowe jest mniejsze od jedynki. Zaokrąglam je do tej wartości. Mnożę razy trzy więc R_th dla każdej osi wychodzi 3. Obliczenia można podejrzeć w pliku.

Cytuj:
Co by było gdybyś pobierał dane dla osi OX i OY z chwili t0, a dla osi OZ z chwili t1 (tu oczywiście kwestia w jakim trybie masz bufor ustawiony)? W LSM9DS0 bardzo użyteczne były wyprowadzenia DRDY (data ready), zawsze miałem pewność, że dane już są, szkoda tylko 1 wyprowadzenia.

Nowe dane są rozpoznawane na podstawie pojawienia się stanu wysokiego na linii DRDY, która jest podłączona do pina uC. Dane odbierane w przerwaniu od zmiany stanu na linii. Tak więc odczyt nowych danych zachodzi w jednym czasie i nie ma możliwości pobrania danych z różnych próbkowań. W przerwaniu również sczytywany stan zegara po czym zerowany.

Cytuj:
Tak duży błąd o którym piszesz nie klasyfikuje się jako dryft z którym też powinieneś się zmierzyć, jestem zdania że tu filtr Kalmana będzie niezbędny.

No właśnie nie wiem czy to jest dryft. Chyba nie bo jak wykonuję obrót o 90 względem osi Z i z powrotem do pozycji początkowej nawet kilka razy to powiedzmy wartość przy powrocie będzie wynosić 0,5 deg, 1deg, 1,5deg itd a jak wykonam później multiobrót to po powrocie jest 30 deg.
Jeśli po uruchomieniu gyro jako pierwszy wykonam multiobrót to też wskazanie przy powrocie jest 30 deg.
Więc czas nie ma tutaj chyba nic do rzeczy czy wykonam multiobrót jako pierwszy w ciągu sekundy po inicjalizacji czy po 10 sekundach i wykonując wcześniej obroty tylko wokół osi Z.

Pomiary wykonywane dla: ODR=47,3 [Hz], FS=2000 [dps]


Załączniki:

Aby zobaczyć załączniki musisz się zalogować. Tylko zalogowani użytkownicy mogą oglądać i pobierać załączniki.



Góra
 Zobacz profil  
cytowanie selektywne  Cytuj  
PostNapisane: 10 maja 2018, o 15:56 
Offline
Użytkownik

Dołączył(a): 05 wrz 2017
Posty: 169
Pomógł: 31

Spróbuj wykonać multiobrót względem dowolnej osi ale powoli ~obr/10s i podeślij odczyty żyroskopu z tej operacji.
Wydaje mi się że kolega [b] Alef2[\b] miał rację i tracisz dane w czasie obrotu. Być może w Twoim kodzie przepełnia się jakaś zmienna co daje podobny efekt.

W plikach które wstawiłeś nie widzę nic nadzwyczajnego żyroskop leżał na twardym nieruchomym podłożu, błędy spoczynkowe masz coś koło OX -2.65, OY +2.32, OZ -1.37 stopnia, a drgania są naprawdę niewielkie. Zastanawiają mnie dane z timera, jeśli dobrze przeliczam TNC (0x51E6) to masz tam coś około 20966 taktów/8MHz = 2.62 ms co daje Fs = 1/2.62 kHz czyli coś w granicach 300-400Hz. Nie wiem jaki masz preskaler mam nadzieje że to w nim tkwi ta rozbieżność, przy 1:8 ODR by się zgadzał.

Dryft żyroskopu oddziałuje na pomiar ale w znacznie mniejszym stopniu i znacznie mniejszej skali; jak byś włączył swój układ na powiedzmy cały dzień i stał by tylko nieruchomo, zapewne zauważył byś, że pozycja spoczynkowa OX =0.0 zmieniła się na np. OX = 0.7 albo coś podobnego (mały procent zakresu pomiarowego). Właśnie z uwagi na powolność tego procesu da się go wyeliminować filtrem górnoprzepustowym.



Góra
 Zobacz profil  
cytowanie selektywne  Cytuj  
PostNapisane: 11 maja 2018, o 16:02 
Offline
Nowy

Dołączył(a): 17 sty 2013
Posty: 10
Pomógł: 0

Wykonałem sprawdzenie czy obliczone kąty względem obrotu wokół każdej osi są poprawne i wyszło że tak. Dane z obliczeń w programie zgadzają się z obliczeniami w exelu. Tak więc problem tkwi w sposobie obliczania kwaternionu z kątów na wyjściu.

Cytuj:
Nie wiem jaki masz preskaler mam nadzieje że to w nim tkwi ta rozbieżność, przy 1:8 ODR by się zgadzał


Tak, preskaler ma wartość 8. Wprowadza to drobny błąd ale na pewno nie taki jaki występuje w moim przypadku.

Skoro dryft odpada to jak pisałem wcześniej błąd jest wyznaczaniu kwaternionu.

Opiszę tok swojego rozumowania.
Na wyjściu z obliczeń dostaję wartości trzech kątów obrotu. Tak więc teoretycznie położenie po obrocie można uzyskać obracając gyro wokół osi X o kąt alfa, następnie wokół osi Y o kąt beta i następnie wokół osi Z o kąt gamma. Ale nie ma tak dobrze bo te obroty w naszym przypadku z gyro zachodzą jednocześnie. Nie możemy określić który obrót był pierwszy i to nie pozwala nam na stosowanie składania obrotów elementarnych. W naszym przypadku w chwili t0 gyro jest zorientowane kątowo alfa0, beta0, gamma0 a w czasie t1 już alfa1, beta2, gamma1. Tak więc znając położenie kątowe po obrocie w trzech osiach można jest zastąpić obrotem wokół jednej osi całkowicie nowo zorientowanej o nowy kąt i do tego celu służy kwaternion. Tylko jak wyznaczyć jego składowe q=(q0, q1, q2, q3). W linku
https://en.wikipedia.or/wiki/Conversion ... Conversion
jest pokazany sposób przejścia z kątów na kwaternion ale dla przejścia pomiędzy obrotami wokół osi Z, Y i X i pewnie dlatego jak wykonuję multiobrót to kolejność obrotów nie zgadza się z założonymi we wzorze.

Znalazłem filmik który pokazuje jak obliczyć kwaternion z gyro dla multiobrotu ale nie widzę jak wyciągnąć te wartości których szukam.
https://www.youtube.com/watch?v=VNIszCO4azs

Tutaj podobnie występuje obliczenie składowych kwaternionu
https://www.matematyka.pl/426906.htm#p5519032

Tutaj też występuje ten wzór
https://folk.uio.no/jeanra/Informatics/ ... dIMUs.html

Proszę o odniesienie się do mojego toku rozumowania oraz o wskazówkę jak rozpisać tą zamianę na kwaternion.



Góra
 Zobacz profil  
cytowanie selektywne  Cytuj  
PostNapisane: 12 maja 2018, o 07:47 
Offline
Nowy

Dołączył(a): 17 sty 2013
Posty: 10
Pomógł: 0

Trochę uporządkowałem pobierane dane. Podsyłam wyniki z pomiarów w których sprawdzałem czy zmiany kąta zgadzają się z tymi generowanymi przez program.
Wszystko jest okej. Można porównać wartości ze screena z konsoli z danymi w pliku.

Więc problem tkwi w obliczaniu kwaternionu. Proszę o sugestie jak dokonać uniwersalnej konwersji z kątów na kwaternion tak aby nie zachodziła potrzeba obmyślania który obrót wykonać jako pierwszy.


Załączniki:

Aby zobaczyć załączniki musisz się zalogować. Tylko zalogowani użytkownicy mogą oglądać i pobierać załączniki.



Góra
 Zobacz profil  
cytowanie selektywne  Cytuj  
PostNapisane: 12 maja 2018, o 15:13 
Offline
Użytkownik

Dołączył(a): 25 lip 2013
Posty: 2561
Pomógł: 126

Pliki pakujemy do zipów.



Góra
 Zobacz profil  
cytowanie selektywne  Cytuj  
PostNapisane: 13 maja 2018, o 20:14 
Offline
Użytkownik

Dołączył(a): 05 wrz 2017
Posty: 169
Pomógł: 31

Pobieżnie przeglądając plik excela wygląda to w miarę normalnie.
Powiem tak, nie widząc Twojego kodu ciężko powiedzieć czy gdzieś jest babol w samym kodzie czy w algorytmie który wymyśliłeś.
Jeśli algorytm jest błędny to może powinieneś przejść na jednorodne macierze transformacji, kiedyś na zajęciach z robotyki przeliczałem to na piechotę oraz w matlabie - teraz już niewiele z tego pamiętam :)
Odezwij się na PW to podeślę Ci skrypt z teorią i pliki matlabowskie, może dzięki temu pójdziesz do przodu. Jeśli to nie pomoże to znaczy że coś w kodzie napaskudziłeś (skrypty do matlaba jeśli dobrze pamiętam rysują kreskowego robota i poruszają ramionami).



Góra
 Zobacz profil  
cytowanie selektywne  Cytuj  
PostNapisane: 14 maja 2018, o 13:30 
Offline
Nowy

Dołączył(a): 17 sty 2013
Posty: 10
Pomógł: 0

Skończyłem dopisywać obliczenia w excelu i końcowe wyniki wychodzą takie same jak w obliczeniach w programie.

Bardzo chętnie bym przeszedł na jednorodne macierze transformacji gdyby nie to że ja nie wiem, który obrót zachodzi pierwszy a to jest niezbędna informacja aby je stosować.

Wykonałem taki multi obrót z położenia bazowego tzn oś Z skierowana pionowo do góry. Obrót zachodził wokół różnych losowych osi. Przeważnie był wykonywany naprzód ale były też drobne powroty. Tak czy inaczej końcowe położenie jest takie same jak początkowe. Obroty wykonywane trzymając gyro rękoma. Pomiary wykonywane dla: ODR=47,3 [Hz], FS=2000 [dps]

Cytuj:
Wydaje mi się że kolega [b] Alef2[\b] miał rację i tracisz dane w czasie obrotu. Być może w Twoim kodzie przepełnia się jakaś zmienna co daje podobny efekt.

To przypuszczenie chyba odpada bo skoro wyniki w excelu są takie same jak z konsoli to nie mogło być żadnego przepełnienia.


Załączniki:

Aby zobaczyć załączniki musisz się zalogować. Tylko zalogowani użytkownicy mogą oglądać i pobierać załączniki.



Góra
 Zobacz profil  
cytowanie selektywne  Cytuj  
PostNapisane: 15 maja 2018, o 19:03 
Offline
Użytkownik

Dołączył(a): 05 wrz 2017
Posty: 169
Pomógł: 31

Sory że tak późno się odzywam ale mam dość dużo pracy na głowie i niestety forum trochę na dalszy plan zeszło.
Z tego co widzę to masz pewnego babola w algorytmie i tu nie ma się nad czym zastanawiać. Możesz powiedzieć co takiego projektujesz że nie znasz początkowego obrotu ?



Góra
 Zobacz profil  
cytowanie selektywne  Cytuj  
PostNapisane: 15 maja 2018, o 21:17 
Offline
Nowy

Dołączył(a): 17 sty 2013
Posty: 10
Pomógł: 0

Ale babola w algorytmie w sensie błąd w obliczeniach czy moje założenia są błędne?

Nie znam początkowego obrotu bo nie wiem wokół której osi nastąpi obrót. Z tego wynika że jedyne poprawne działanie gyro będzie gdy w jednym odczycie będzie zachodzić obrót wokół jednej osi tak? Jeśli zajdą już dwa obroty wokół dwóch osi to przecież nie jesteśmy w stanie określić który zaszedł pierwszy bo na wyjściu otrzymujemy dane jednocześnie.

Więc jak do tego podejść?



Góra
 Zobacz profil  
cytowanie selektywne  Cytuj  
PostNapisane: 18 maja 2018, o 07:17 
Offline
Nowy

Dołączył(a): 17 sty 2013
Posty: 10
Pomógł: 0

Wykonałem obliczenia dla wszystkich sześciu możliwych kombinacji kolejności osi obrotów mianowicie: XYZ, XZY, YXZ, YZX, ZXY, ZYX. Liczyłem to składając elementarne osie obrotu w jedną i następnie mnożąc razy wektor którego położenie po obrocie poszukuję. Wyglądało to tak np: Rx*Ry*Rz*P=P'. Mnożenie macierzy występowało od lewej strony tzn po pierwszym mnożeniu było Rxy*Rz*P=P',
Rxyz*P=P'. Następnie do dalszych obliczeń przy kolejnych obrotach brana była wartość P' tzn Rx*Ry*Rz*P'=P'' itd itd
Wyniki są prawie zgodne tzn największa różnica wyniosła około 1 stopnia. Wyniki obliczeń pokrywają się także z tym co wyszło mi z obliczeń w programie. Ale ponownie nie są zgodne z tym co powinno wystąpić tzn kąt wychyłu osi Z od pionu nie powraca do wartości zero stopni.

Bardzo proszę jeśli ktoś wykonywał takie obliczenia albo w excel albo w matlabie o podesłanie arkusza obliczeniowego bo chciałbym podstawić swoje dane do jakiegoś skryptu aby sprawdzić czy to ja robię gdzieś błąd w obliczeniach czy może jest gdzieś błąd w danych zwracanych przez gyro.
Albo proszę o opis wszystkich kroków jakie zachodzą podczas procesu wyznaczania nowego położenia układu współrzędnych względem układu bazowego. Konkretnie chodzi mi od odchylenia kątowe osi Z od pionu.



Góra
 Zobacz profil  
cytowanie selektywne  Cytuj  
Wyświetl posty nie starsze niż:  Sortuj wg  
Utwórz nowy wątek Odpowiedz w wątku  [ Posty: 14 ] 

Strefa czasowa: UTC + 1


Kto przegląda forum

Użytkownicy przeglądający ten dział: Brak zidentyfikowanych użytkowników i 6 gości


Nie możesz rozpoczynać nowych wątków
Nie możesz odpowiadać w wątkach
Nie możesz edytować swoich postów
Nie możesz usuwać swoich postów
Nie możesz dodawać załączników

Szukaj:
Skocz do:  
Sitemap
Technologię dostarcza phpBB® Forum Software © phpBB Group phpBB3.PL
phpBB SEO