Analiza działania systemu kursowego (symulacja) – instrukcja

W O J S K O W A A K A D E M I A T E C H N I C Z N A im. Jarosława Dąbrowskiego

Zintegrowane Systemy Nawigacyjne

Ćwiczenie laboratoryjne:

Analiza działania systemu kursowego

Warszawa, 2020

2 1. Cel ćwiczenia

Celem ćwiczenia jest symulacja systemu nawigacji zliczeniowej wykorzystującego giroskop do określenia kursu obiektu. W ramach ćwiczenia należy zaimplementować wybrane metody całkowania numerycznego (w środowisku Matlab) oraz ocenić dokładność uzyskiwanych rezultatów.

2. Błędy czujników inercjalnych

Wszystkie typy przyspieszeniomierzy i giroskopów wykazują obecność błędów takich jak: obciążenie (ang. bias), błędy współczynnika skali (ang. scale factor), błędy sprzężenia osi pomiarowych (ang. cross- coupling errors) oraz błędy o charakterze losowym. Błędy wyższych rzędów oraz wzajemne sprzężenie pomiędzy mierzoną prędkością kątową i przyspieszeniem (ang. angular rate-specific force cross- sensitivity) mogą się pojawić w zależności od rodzaju czujnika.

Błędy systematyczne mają cztery składowe: stałą, składnik zależny od temperatury, składnik zmienny od włączenia do włączenia (ang. run to run) oraz składnik zmienny podczas pracy urządzenia (ang. in-run).

Składowa stała jest obecna każdorazowo podczas pracy czujnika i jest najczęściej kompensowana przez procesor IMU na podstawie danych kalibracyjnych zebranych podczas badań laboratoryjnych czujnika.

Podobna sytuacja ma miejsce w przypadku błędu zależnego od temperatury. Jeśli korekcja nie jest wykonana czujnik wykazuje zmianę błędu systematycznego przez kilka minut po uruchomieniu, aż do ustabilizowania jego temperatury.

Składnik zmienny od włączenia do włączenia przyjmuje inną wartość po każdym uruchomieniu czujnika, jednak podczas jego pracy pozostaje stały. Nie może być skorygowany przez procesor IMU lecz może być korygowany przez INS lub algorytm integracji danych pomiarowych.

Składnik zmienny podczas pracy systemu zmienia się powoli podczas pracy czujnika. Nie może być skorygowany przez IMU.

Obciążenie (ang. bias) jest błędem systematycznym dotyczącym zarówno przyspieszeniomierzy jak i giroskopów. Jest ono niezależne do przyłożonego sygnału wejściowego. W większości przypadków obciążenie jest dominującą składową całkowitego błędu czujników nawigacyjnych. Czasami błąd ten jest nazywany błędem g-independent. Obciążenia przyspieszeniomierzy i giroskopów po kalibracji i kompensacji mogą być zapisane jako wektory: b

= (b

, b

, b

) oraz b

= (b

,b

,b

). Błędy czujników są z reguły określane w osiach pomiarowych IMU. Obciążenie można podzielić na składową statyczną oraz dynamiczną: b

= b

+ b

, b

= b

+ b

.

Rys. 1. Obciążenie (bias) i jego wpływ na wynik pomiaru

3

Składnik statyczny, nazywany także fixed bias, turn-on bias lub bias repeatability, składa się z obciążenia zmieniającego się od włączenia do włączenia oraz resztowych, stałych błędów pozostałych po procesie kalibracji. Jest stały podczas pracy, ale zmienia się od włączenia do włączenia. Składnik dynamiczny, nazywany in-run bias variation lub bias instability, zmienia się z okresem rzędu minuty.

Stanowi on około 10% wartości składnika statycznego. Jest on najlepiej mierzalny za pomocą wariancji Allana – jest to jej punkt minimalny. Im niższa wartość tym lepiej. Błąd ten trudno usunąć w drodze kalibracji.

Obciążenia przyspieszeniomierzy i giroskopów nie są z reguły wyrażane w jednostkach układu SI. W przypadku przyspieszeniomierzy wykorzystuje się 𝑚𝑔 lub 𝑢𝑔. W przypadku giroskopów: 𝑑𝑒𝑔/ℎ, lub w przypadku czujników niskiej jakości 𝑑𝑒𝑔/𝑠.

Przyspieszeniomierze MEMS wykazują obciążenie 3 − 10 𝑚𝑔. Giroskopy RLG mają obciążenie rzędu 0,001 𝑑𝑒𝑔/ℎ, jednak niskokosztowe RLG mają obciążenia rzędu 10 𝑑𝑒𝑔/ℎ, natomiast giroskopy FOG: od 0,01 do 100 𝑑𝑒𝑔/ℎ. W nieskalibrowanych czujnikach MEMS występują także znaczne błędy zależne od temperatury, wynoszące kilka 𝑑𝑒𝑔/𝑠 lub kilkanaście 𝑚𝑔.

Błąd współczynnika skali polega na zmianie współczynnika wzmocnienia pomiędzy sygnałem wejściowym a wyjściowym, który powinien wynosić jeden. W przypadku przyspieszeniomierzy wynikowy błąd jest proporcjonalny do przyspieszenia pozornego, natomiast w giroskopach zależy od prędkości kątowej wokół osi czułości. Błędy te zapisywane są jako wektory: 𝒔

= (𝑠

, 𝑠

, 𝑠

), 𝒔

= (𝑠

, 𝑠

, 𝑠

).

Sprzężenia skrośne pomiędzy osiami są wynikiem braku ortogonalności osi czułości czujników w IMU, najczęściej wynikają z ograniczeń technologicznych. W ich wyniku przyspieszeniomierz jest czuły na przyspieszenia pozorne w osiach prostopadłych do jego osi czułości, podobnie giroskop jest czuły na obroty realizowane wokół osi prostopadłych do jego osi czułości. Błędy orientacji osi powodują także dodatkowy błąd skali, lecz jest on o dwa-cztery rzędy wielkości mniejszy od błędów sprzężenia skrośnego.

W czujnikach wibracyjnych sprzężenie skrośne może wynikać także, z „przeciekania” drgań pomiędzy czujnikami (ang. cross-talk). W konsumenckich czujnikach MEMS błąd sprzężenia skrośnego może osiągać wartości porównywalne z błędem wynikającym z niewłaściwego montażu czujnika.

Rys. 2. Wpływ błędu współczynnika skali na sygnał wyjściowy czujnika

Symbolem 𝑚

oznacza współczynnik sprzężenia skrośnego pomiędzy przyspieszeniem pozornym

w osi β a przyspieszeniem widocznym w przyspieszeniomierzu osi α, analogicznie 𝑚

oznacza

sprzężenie skrośne między osiami giroskopu

Błędy współczynników skali oraz błędy skrośne w przypadku

triady przyspieszeniomierzy i giroskopów można zapisać w postaci macierzy:

4

Błędy przyspieszenia pozornego lub prędkości kątowej wyznaczane są zatem z zależności: M

f

oraz M

𝛚

.

Błędy skali oraz sprzężenia skrośnego są bezwymiarowe, opisywane najczęściej w 𝑝𝑝𝑚 (ang. parts per million) lub w %. Dla większości czujników inercjalnych błędy skali i sprzężenia skrośnego są w przedziale 1𝑒 − 4 . . 1𝑒 − 3 (100. .1000 𝑝𝑝𝑚). Są od tego dwa główne wyjątki: nieskalibrowane konsumenckie czujniki MEMS mogą mieć błąd skali wynoszący 10% i sprzężenie skrośne 2%. RLG wykazują najmniejsze błędy skali: 1𝑒 − 6. .1𝑒 − 4 (1 − 100𝑝𝑝𝑚).

Błędy losowe

Pomiary wykonywane przez czujniki inercjalne obarczone są błędami o charakterze losowym. Błędy te zapisywane są w postaci wektorów 𝒘

= (𝑤

, 𝑤

, 𝑤

) oraz 𝒘

= (𝑤

, 𝑤

, 𝑤

).

Widmo szumu przyspieszeniomierzy i giroskopów dla częstotliwości poniżej 1Hz jest w przybliżeniu białe, dlatego odchylenie standardowe przyspieszenia i prędkości kątowej jest odwrotnie proporcjonalne do pierwiastka czasu uśredniania. Szum czujników IMU jest zatem wyrażany najczęściej jako pierwiastek widmowej gęstości mocy (and. Power Spectral Density – PSD). Jednostkami są najczęściej 𝜇𝑔/√𝐻𝑧 oraz

°/ℎ𝑟/√𝐻𝑧. Odchylenie standardowe szumu jest uzyskiwane przez pomnożenie pierwiastka PSD przez pierwiastek z częstotliwości wykonywania pomiarów. Biały szum nie może być usunięty w drodze kalibracji.

Szum pomiarów przyspieszenia pozornego po całkowaniu prowadzi do błędu velocity random walk (VRW). Analogicznie szum pomiarów prędkości kątowej po całkowaniu prowadzą do błędu attitude random walk (ARW). Odchylenie standardowe procesów random walk jest proporcjonalne do pierwiastka kwadratowego czasu integracji.

ARW opisuje średnie odchylenie lub błąd, który pojawi się w wyniku szumu. Może być określony na

podstawie wariancji Allana w punkcie 1-sec. Dla krótkich czasów uśredniania szum czujnika jest

dominujący (zbocze nachylone -1/2). Głównymi źródłami szumu są elementy aktywne takie jak dioda

laserowe, fotodioda w FOG oraz silikonowa lub kwarcowa drgająca belka w MEMS.

5

Rys. 3. Wykres wariancji Allana

W przypadku przyspieszeniomierzy pierwiastek PSD zmienia się od około 20 𝜇𝑔/√𝐻𝑧 dla urządzeń lotniczych, poprzez 100 𝜇𝑔/√𝐻𝑧 dla urządzeń taktycznych do 1000 𝜇𝑔/√𝐻𝑧 dla czujników MEMS. W przypadku giroskopów szum charakteryzuje się wartościami rzędu 0,001-0,02 °/√ℎ. Giroskopy FOG klasy taktycznej wykazują szum o pierwiastku PSD w zakresie 0,03-0,1 °/√ℎ, MEMS – 0,06-2°/√ℎ.

Źródłem błędów losowych jest także ograniczona długość słowa wyjściowego (szum kwantyzacji).

Polega on na ograniczeniu sygnału wyjściowego do wartości całkowitej mnożonej przez stałą, tzw. poziom kwantyzacji W IMU klasy taktycznej słowo wyjściowe ma długość przeważnie 16 bitów, natomiast w czujnikach konsumenckich jest to przeważnie 8-12 bitów.

Rys. 4. Wpływ błędu kwantyzacji na sygnał wyjściowy czujnika

Przyspieszeniomierze i giroskopy wykazują także dalsze błędy w zależności od typu czujnika.

Giroskopy wibracyjne, klasyczne oraz FOG wykazują czułość na przyspieszenie pozorne (ang. g- dependent errors).

Czujniki inercjalne wykazują także nieliniowość współczynnika skali (ang. scale factor nonlinearity), co oznacza, że współczynnik skali zmienia się w zależności od przyłożonej siły lub prędkości obrotowej.

Największe zmiany współczynnika skali obserwowane są najczęściej przy pomiarze największych

przyspieszeń i prędkości kątowych.

6

Rys. 5. Wpływ nieliniowości współczynnika skali na sygnał wyjściowy czujnika

Część przyspieszeniomierzy MEMS i wibracyjnych przyspieszeniomierzy wykazuje zmienność sprzężenia skrośnego pomiędzy osiami w zależności od przyłożonej siły lub prędkości kątowej. Dzieje się tak w wyniku zmiany położenia osi czułości w wyniku przyłożonej siły.

Czujniki inercjalne są podatne także na błędy wynikające z drgań, które powodują zmiany współczynników skali i sprzężenia skrośnego. W długim czasie błędy te wykazują zerową wartość średnią.

Jednakże niesymetrie lub nieliniowości współczynników skali i sprzężenia skrośnego powodują, że błąd ten się nie zeruje. Zjawisko to znane jest jako Vibration Rectification Error, który zachowuje się jak obciążenie zmieniające się wraz z amplitudą drgań. Błąd ten dotyczy przede wszystkim giroskopów wibracyjnych i przyspieszeniomierzy MEMS. Błędy o charakterze oscylacyjnym powodują powstanie w systemie INS także błędów coning i sculling.

3. Metody całkowania numerycznego

Wartość kąta jest wyznaczana poprzez całkowanie prędkości kątowej mierzonej przez giroskop.

Całkowanie numeryczne może być realizowane różnymi metodami. W toku realizacji ćwiczenia laboratoryjnego należy wykorzystać całkowanie:

• metodą prostokątów,

• metodą trapezów.

3.1. Całkowanie metodą prostokątów

W metodzie tej funkcja podcałkowa f ( ) x interpolowana jest za pomocą wielomianu stopnia zerowego

( ) ^x

^{= const}

f _t (1)

zatem:

( ) x dx f ( ) ( x

b a ) ( ) f x

b

f

a

b

a

→ −

 =  ⁽²⁾

W zależności od wyboru położenia węzła x

otrzymujemy wzory:

a) lewych prostokątów (metoda prostokątów w przód), gdy x =

a ,

7

b) prawych prostokątów (metoda prostokątów wstecz), gdy x =

b ,

3.2. Całkowanie metodą trapezów

Jeśli do interpolacji funkcji podcałkowej f ( ) x zastosujemy interpolację za pomocą wielomianu stopnia pierwszego, wówczas:

( ) ( ) ( ) ^dx ( ^{b a} ) ( ) ( )  ^{f a f b} 

a b

a b x b f a

b a x f dx x

b

f

a

b

a

→ − +

 

 

− + −

−

= −

  ₂ ⁽³⁾

4. Opis systemu pomiarowego

Symulacja działania 3-osiowego giroskopu realizowana jest z wykorzystaniem środowiska Matlab z zainstalowaną biblioteką Navigation Toolbox. Parametry giroskopu ustalane są za pomocą struktury gyroparams. Za jej pośrednictwem możliwe jest wprowadzenie wartości następujących parametrów:

• zakres pomiarowy (MeasurementRange), 𝑟𝑎𝑑/𝑠

• rozdzielczość (Resolution), (rad/s)/LSB

• składnik statyczny obciążenia (ConstantBias), 𝑟𝑎𝑑/𝑠

• widmowa gęstość mocy szumu czujnika (NoiseDensity), (

𝑠

) /√𝐻𝑧

• składnik dynamiczny obciążenia (BiasInstability), 𝑟𝑎𝑑/𝑠

• błądzenie losowe (RandomWalk), (

𝑠

) √𝐻𝑧

• obciążenie temperaturowe (TemperatureBias), (

𝑠

) /℃

• błąd współczynnika skali, zależny od temperatury (TemperatureScaleFactor), %/℃

• błąd pomiaru prędkości kątowej wynikający z przyspieszenia (AccelerationBias), rad/s)/(m/s^2).

4.1. Generowanie danych pomiarowych

Do generowania danych pomiarowych pochodzących z 3-osiowego giroskopu należy wykorzystać skrypt generate_gyro.m (uwaga – skrypt do prawidłowego działania wymaga zainstalowania biblioteki Navigation Toolbox, jest ona dostępna dla środowiska Matlab, od wersji 2019b wzwyż, jeśli nie posiadasz zainstalowanej tej biblioteki istnieje możliwość wykorzystania danych wygenerowanych wcześniej przez prowadzącego ćwiczenie). Wynikiem symulacji są dane pomiarowe (prędkość kątowa) zmierzone przez giroskop poddany zadanej prędkości kątowej.

W skrypcie, w sekcji Data generation, użytkownik ma możliwość zdefiniowania:

• nazwy pliku, do którego trafią dane wynikowe (zmienna output_filename),

• funkcji służącej do wygenerowania danych pomiarowych (zadana prędkość kątowa zapisana jest w macierzy angvel),

• częstotliwości wykonywania pomiarów przez giroskop (zmienna Fs)

• liczby wygenerowanych próbek (zmienna N).

8

Z liczby próbek i częstotliwości próbkowania wynika pośrednio czas wykonywania pomiarów.

Przykładową treść skryptu, w którym zadana prędkość kątową zmienia się zgodnie z funkcją sinus, częstotliwość próbkowania wynosi 100 Hz, zaś liczba wygenerowanych próbek to 1000, przedstawiono na listingu 1.

%% Prepare workspace:

clc

close all clear all

%% Data generation:

output_filename = 'gyro.mat';

params = gyroparams;

% Generate N samples at a sampling rate of Fs with a sinusoidal frequency

% of Fc.

N = 1000;

Fs = 100;

Fc = 0.25;

t = (0:(1/Fs):((N-1)/Fs)).';

acc = zeros(N, 3);

angvel = zeros(N, 3);

angvel(:,1) = sin(2*pi*Fc*t);

disp("Simulation...")

imu = imuSensor('SampleRate', Fs, 'Gyroscope', params);

% Define gyro error here:

% …

[~, gyro_data] = imu(acc, angvel);

disp(" finished")

save(output_filename,'gyro_data', 'angvel', 't')

%% Plots

disp("Figures generation...") figure

plot(t, angvel(:,1), '--', t, gyro_data(:,1)) xlabel('Time (s)')

ylabel('Angular Velocity (rad/s)') title('Ideal Gyroscope Data')

legend('x (ground truth)', 'x (gyroscope)') disp(' finished')

List. 1. Przykładowa treść skryptu generate_gyro.m

Zmienna angvel to macierz o wymiarach Nx3, gdzie poszczególne kolumny odpowiadają zadanym prędkościom kątowym w osiach OX, OY i OZ czujnika, zaś gyro_data macierz o wymiarach Nx3, gdzie kolumny zawierają wyniki pomiarów prędkości kątowych w wymienionych osiach.

W celu włączenia w symulacji wybranego źródła błędów należy przed wywołaniem funkcji imu() wprowadzić zadaną wartość błędu, przykładowo:

imu.Gyroscope.RandomWalk = 9.1e-2; % (rad/s)*sqrt(Hz)

9 5. Instrukcja ćwiczenia laboratoryjnego 5.1. Całkowanie numeryczne

• Wczytać dane pomiarowe pochodzące z idealnego giroskopu. Prędkość kątowa wokół osi OX dana jest zależnością (4), pozostałe składowe wektor prędkości są zerowe.

𝛚

= 𝑠𝑖𝑛 ( 2𝜋𝐹

𝐭 ) (4)

𝛚

to zadany wektor prędkości kątowej wokół osi OX (wymiary N x 1) [rad/s], 𝐹

to częstotliwość sygnału prędkości [Hz], zaś t to wektor zawierający czasy wykonania pomiarów:

𝑡 ∈ 〈0, (𝑁 − 1)

𝐹

〉 (5)

Parametry symulacji:

- 1000 pomiarów (𝑁 = 1000),

- częstotliwość próbkowania 100 Hz (𝐹

= 100),

- częstotliwość symulowanego sygnału prędkości wynosi 0,25 Hz (𝐹

= 0.25).

Wyniki, w postaci zadanej prędkości kątowej (macierz angvel), mierzonej prędkości kątowej (gyro_data) i czasu pomiarów (t), zapisano w pliku ideal_gyro.mat.

• Napisać funkcje realizujące całkowanie numeryczne sygnału zadanego jako argument wywołania funkcji i zapisać je w m-plikach: całkowanie metodą prostokątów w przód:

rect_forward.m, całkowanie metodą prostokątów w tył – rect_backward.m, całkowanie metodą trapezów – trapezoid.m. Wskazówka – aby wykonać całkowanie do funkcji należy zadać wektor wartości funkcji f(t) i wektor czasu t, w którym wykonano pomiary. Każda z funkcji powinna przetwarzać dane pomiarowe w pętli, wyznaczać całkę sygnału dla jednego okresu pomiarowego i takie wyniki zapisywać w macierzy stanowiącej wynik działania funkcji (w tym przypadku numer indeksu wynikowej macierzy odpowiada numerowi całkowanego okresu pomiarowego). W wyniku całkowania wektora prędkości kątowej otrzymamy więc wektor zmiany kątów w poszczególnych okresach pomiarowych.

Sumując te zmiany i dodając je do początkowej wartości kąta otrzymamy wartość kąta w danej chwili pomiarowej.

• Napisać skrypt integration.m wczytujący wygenerowane dane pochodzące z symulacji

giroskopu (do wczytania danych z pliku nazwa_pliku.mat wykorzystaj metodę

load(nazwa_pliku.mat), np. load(ideal_gyro.mat)). W skrypcie dokonaj całkowania

numerycznego sygnału zaimplemontownymi trzema metodami. Wyznacz wartości kąta w

chwilach pomiarowych (wskazówka – dokonaj sumowania wyznaczonych zmian kąta

względem kąta początkowego: 0 rad). Przykładowe wyniki całkowania przedstawiono na

rys. 6-7.

10

Rys. 6. Orientacja kątowa obiektu Rys. 7. Orientacja kątowa obiektu - powiększony fragment wykresu

• W sprawozdaniu zamieść opracowane m-skrypty oraz otrzymane wykresy.

5.2. Analiza wpływu błędów giroskopu na wyniki pomiaru kąta

Zadanie polega na przeanalizowaniu wpływu wybranego źródła błędów (wymienionych w tab. 1) na wynik pomiaru kąta. Należy opracować m-sktypt, a w nim:

• wczytać dane pomiarowe pozbawione błędów z pliku (ideal_gyro.mat), wczytane zmienne przepisać do zmiennych ideal_nazwa_zmiennej, np. gyro_data przypisać do zmiennej ideal_gyro_data itd.

• wczytać dane pomiarowe z obecnym, aktywnym jednym źródłem błędów z pliku (typ_błędu_gyro.mat), zgodnie z tab. 1, wczytane zmienne przepisać do zmiennych dirty_nazwa_zmiennej, np. gyro_data przypisać do zmiennej dirty_gyro_data itd.

• wykonać całkowanie metodą trapezów danych idealnych i obarczonych błędem, wyniki zapisać w zmiennych ideal_angle i dirty_angle.

• na wspólnym wykresie przedstawić wartości zmiennych ideal_angle i dirty_angle w funkcji czasu,

• na wykresie przedstawić błąd wyznaczania kąta w funkcji czasu (błąd to różnica pomiędzy wartością ideal i dirty),

• na wspólnym wykresie przedstawić wartości zadanej prędkości kątowej (pierwsza kolumna macierzy angvel) i zmierzonej prędkości kątowej (pierwsza kolumna macierzy dirty_gyro_data)

• wnioski

Tab. 1. Opis wygenerowanych plików symulacyjnych

L.p. Nazwa pliku

Nazwa parametru/błędu (Matlab)

Wartość

1 mr_gyro.mat MeasurementRange zakres pomiarowy giroskopu ograniczony do 0.5 rad/s

2 r_gyro.mat Resolution rozdzielczość giroskopu ograniczona do 0.5 𝑟𝑎𝑑/𝑠/𝐿𝑆𝐵

3 cb_gyro.mat ConstantBias wprowadzono stałe obciążenie w osi OX: [0.4 0 0]

𝑟𝑎𝑑/𝑠

11

4 nd_gyro.mat NoiseDensity Wprowadzono szum czujnika o widmowej gęstości mocy wynoszącej 1.25e-2

( 𝑟𝑎𝑑

𝑠 ) /√𝐻𝑧

5 bi_gyro.mat BiasInstability Wprowadzono składnik dynamiczny obciążenia wynoszący 2.0e-2

𝑟𝑎𝑑/𝑠

6 tb_gyro.mat TemperatureBias Wprowadzono zmianę obciążenia zależną od temperatury wynoszącą 0.06 (

𝑠

) /℃. Symulację przeprowadzono dla T=3℃5. Temperatura

nominalna wynosi 25℃.

7 tsf_gyro.mat TemperatureScaleFactor Wprowadzono zależność współczynnika skali od temperatury, zależność ta wynosi 3.2

%/℃. W symulacji temperatura czujnika zmienia

się liniowo w czasie od wartości 25℃, (wsp. b) ze

współczynnikiem kierunkowym a wynoszącym

2℃: 𝑓(𝑡) = 𝑎𝑡 + 𝑏