Instrukcje laboratoryjne

1

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 INS

autor instrukcji: mgr inż. Michał Łabowski

Warszawa, 2017

2 1. Opis platformy pomiarowej

System nawigacyjny składa się z dwóch przyrządów pomiarowych: IMU KVH 1750 oraz zintegrowanego systemu nawigacyjnego INS/GNSS SBG Ekinox-D. Czujnik IMU KVH 1750 jest inercjalną jednostką pomiarową klasy taktycznej, składającą się z trzech przyspieszeniomierzy MEMS i trzech giroskopów FOG.

Danymi wyjściowymi z urządzenia są: wektor przyspieszenia pozornego oraz wektor zmiany kątów orientacji przestrzennej.

Ekinox-D firmy SBG jest zintegrowanym systemem nawigacyjnym INS/GNSS, posiadającym dwuantenowy odbiornik GNSS. Część inercjalna urządzenia SBG składa się z trzech przyspieszeniomierzy oraz trzech giroskopów typu MEMS. Odbiornikiem GNSS zabudowanym wewnątrz czujnika Ekinox jest model OEM-615 firmy Novatel. Jest to odbiornik dwuantenowy z możliwością uwzględniania poprawek RTK. Pozwala on na śledzenie sygnałów L1 i L2 konstelacji GPS oraz sygnałów L1 i L2 konstelacji GLONASS. Dzięki dwóm antenom możliwe jest określenie kursu geograficznego (KG) obiektu, czyli kąta pomiędzy osią anten (która pokrywa się z osią podłużną BSP) a lokalnym południkiem geograficznym.

Lokalny układ odniesienia związany z samolotem oznaczono jako b-frame (ang. body frame). Początek układu – punkt 𝑂

^𝑏

znajduje się w pobliżu środka ciężkości obiektu. Oś 𝑂𝑋

^𝑏

układu b-frame skierowana jest wzdłuż osi podłużnej BSP, tj. w stronę jego części przedniej, oś 𝑂𝑌

^𝑏

skierowana jest w stronę prawego skrzydła, natomiast oś 𝑂𝑍

^𝑏

dopełnia układ prawoskrętny. Z przyrządami KVH i SBG związane są z kolei układy lokalne nazwane odpowiednio s1-frame oraz s2-frame (s-frame od ang. sensor frame), co przedstawiono na rys.1. Osie układu odniesienia związanego z czujnikiem IMU (s1-frame) posiadają kierunki i zwroty zgodne z kierunkami i zwrotami jego osi pomiarowych.

Rys. 1. Uogólniony schemat samolotu z rozmieszczeniem urządzeń nawigacyjnych i radarowych

Badania opisanego systemu przeprowadzono z wykorzystaniem naziemnej platformy kołowej

przedstawionej na rys. 2.

3

Rys. 2. Testy naziemne podsystemu nawigacyjnego - platforma kołowa

2. Opis wyników pomiarów IMU i GPS

W ćwiczeniu wykorzystane zostaną wyniki pomiarów wykonanych przez przyspieszeniomierze i giroskopy IMU KVH (plik imu.mat) oraz odbiornik GPS (plik gps.mat). Wyniki poszczególnych pomiarów zapisywane są w wektorach kolumnowych, których opis przedstawiono w tab. 1.

Tab. 1. Opis wybranych kolumn macierzy z wynikami pomiarów

Czujnik Kolumna Opis

IMU

8 czas danych [µs od początku doby]

9 przyspieszenie pozorne w osi OX s-frame [g]

10 przyspieszenie pozorne w osi OY s-frame [g]

11 przyspieszenie pozorne w osi OZ s-frame [g]

12 zmiana kąta wokół osi OX s-frame [rad]

13 zmiana kąta wokół osi OY s-frame [rad]

14 zmiana kąta wokół osi OZ s-frame [rad]

GPS

8 czas danych [µs od początku doby]

9 szerokość geograficzna [deg]

10 długość geograficzna [deg]

11 wysokość [m n.p.m.]

20 kurs [rad]

3. Macierz kosinusów kierunkowych

Macierz DCM (ang. Direction Cosine Matrix) transformacji wektora z układu 𝛼-frame do β-frame, dana jest zależnością:

 







 













−



 







 











−





 







 













−



=























cos sin

0

sin cos

0

0 0

1 cos

0 sin

0 1 0

sin 0 cos

1 0

0

0 cos

sin

0 sin

cos

1 1

bs bs

C

₍₁₎

4

Macierz ta składa się z trzech macierzy obrotów, z których pierwszy wykonywany jest obrót układu β-frame wokół jego osi OZ o kąt Ψ

_βα

, następnie wykonywany jest obrót wokół osi OY układu powstałego w kroku poprzednim oraz obrót wokół osi OX. W wyniku przeprowadzenia tych trzech obrotów uzyskuje się pokrycie osi układu β-frame z osiami układu 𝛼-frame.

Mając macierz 𝐂

_α^β

można dokonać przeliczeń w kierunku odwrotnym, tzn. wyznaczyć wartości kątów Eulera:

(

 ^

)





=

 atan2 C

_3,2

, C

_{3,3 (2)}

( )

^



= −

 arcsin C

_{3,1 (3)}

(

 ^

)





=

 atan2 C

_2,1

, C

_{1,1 (4)}

4. Inicjalizacja INS

W nawigacji inercjalnej wyznaczane są zmiany prędkości, zmiany położenia i zmiany orientacji kątowej, jednak w celu określenia bieżących wartości tych elementów nawigacyjnych wymagane jest przyjęcie ich początkowych wartości względem których realizowane jest następnie sumowanie.

4.1. Poziomowanie

W przypadku inicjalizacji wykonywanej w warunkach stacjonarnych, podczas tzw. ciszy mechanicznej (wyłączony silnik i inne elementy mechaniczne, brak oddziaływania ze strony załogi/personelu obsługującego) możliwe jest wyznaczenie początkowych wartości kątów pochylenia Θ

_𝑛𝑏

i przechylenia Φ

_𝑛𝑏

na podstawie pomiarów przyspieszenia pozornego:

(

is^b z

)

b y is

nb

= arctan 2 − f

1,

, − f

1,



₍₅₎

( ) ( ) ^{ }







 







= +



2

, 1 2 , 1

, 1

f f

arctan f

b z is b

y is

b x is

nb (6)

gdzie: [f

_{𝑖𝑠1,𝑥}^𝑏

f

_{𝑖𝑠1,𝑦}^𝑏

f

_{𝑖𝑠1,𝑧}^𝑏

]

^𝑇

to wektor przyspieszenia pozornego układu s1-frame, względem i-frame, wyrażony w b-frame.

4.2. Kurs początkowy

Inicjalizacja kursu ruchu obiektu może być realizowana poprzez pomiar składowych wektora natężenia pola magnetycznego Ziemi, lub za pomocą dwuantenowego odbiornika GPS. W tym drugim przypadku anteny umieszcza się najczęściej wzdłuż osi podłużnej pojazdu. Jedna z anten jest tzw. anteną główną (ang.

master) i to dla niej wyznaczane są pseudoodległości do satelitów, druga z anten (ang. slave) pełni funkcję pomocniczą (wyznaczanie kursu).

4.3. Początkowe położenie i prędkość

Położenie i prędkość początkowa określane są najczęściej za pomocą odbiornika GPS, lub wpisywane są przez użytkownika w przypadku, w którym warunki początkowe są znane.

5. Obliczenia nawigacyjne

Obliczenia wykonywane w systemie nawigacji inercjalnej można podzielić na cztery etapy, co

przedstawiono na rys. 3. Celem obliczeń aktualizacji orientacji przestrzennej jest wyznaczenie aktualnej

5

postaci macierzy 𝐂

_𝑏^𝑛

oraz kątów pochylenia Θ

_𝑛𝑏

, przechylenia Φ

_𝑛𝑏

i odchylenia Ψ

_𝑛𝑏

. Dane wejściowe do tej procedury stanowi wektor zmiany orientacji kątowej mierzony przez giroskopy Δ𝚯

_𝑖𝑠1^𝑠1

oraz wartość macierzy DCM z chwili 𝑘 − 1 𝐂

_{𝑏,𝑘−1}^𝑛

.

Rys. 3. Schemat przepływu danych w INS

Dzięki znajomości aktualnej macierzy 𝐂

_𝑏^𝑛

oraz macierzy 𝐂

_𝑠^𝑏

możliwe jest przeliczenie wektora przyspieszenia pozornego mierzonego przez przyspieszeniomierze f

_𝑖𝑠1^𝑠1

do układu b-frame oraz n-frame. Na podstawie uzyskanego wektora f

_𝑖𝑠1^𝑛

oraz poprzedniego wektora prędkości v

_𝑘−1^𝑛

wyznaczana jest prędkość aktualna. Aktualne położenie (wyrażone w układzie ECEF – szerokość geograficzna φ, długość geograficzna λ oraz wysokość nad elipsoidą odniesienia) obliczane jest poprzez całkowanie wektora prędkości przy znanym położeniu wyznaczonym w kroku poprzednim.

5.1. Aktualizacja orientacji przestrzennej Aktualna macierz 𝐂

_𝑏^𝑛

wyznaczana jest z zależności:

( )

ⁿbk

^t

n en n

ie k

b k b n

k b n

k

b_,

 C

_{, −1}

C

^,_{, −1}

− Ω + Ω C

_{, −1}



C

(7)

gdzie 𝐂

_{𝑏,𝑘−1}^𝑏,𝑘

to macierz aktualizacji orientacji przestrzennej, opisująca przekształcenie macierzy 𝐂

_𝑏^𝑛

z chwili 𝑘 − 1 do chwili 𝑘. Macierz tą opisana jest równaniem:

  _

^

 

− =



= 





=

0 1 1

, 1

,

exp !

r

b r b is

is k

b k

b

r

Θ Θ

C

(8)

Rozwinięcie równania (8) w szereg Tylora ogranicza się najczęściej do wyrazu związanego z wielomianem stopnia czwartego:

  

1



²

2 1 2

1 3

, 1

,

2 24

1

1 6  















 

− +



 













 

− +

−

=

b

is b

b is is b

k is b

k

b

Θ Θ

Θ Θ I

C

₍₉₎

Uzyskana postać jest zbliżona do tzw. równania Rodrigues’a wykorzystującego funkcje trygonometryczne:

 

2



1



²

1 1

1 1 3

, 1 ,

cos 1

sin  



 + −



  + 

−

=

b

b is is

b b is

b is is b k is

b k

b

Θ

Θ Θ Θ

Θ Θ I

C

₍₁₀₎

6

Występująca w równaniu (7) macierz Ω

_𝑖𝑒^𝑛

to postać antysymetryczna wektora prędkości kątowej Ziemi względem układu i-frame (ang. Earth rate) 𝛚

_𝑖𝑒^𝑛

, który określa równanie:

( ) ( ) ^{ }







 













=

sin 0 cos

ie n

ω

ie (11)

gdzie φ stanowi szerokość geograficzną położenia IMU, natomiast ω

_𝑖𝑒

jest prędkością ruchu obrotowego Ziemi:

s 10 rad 292115 ,

7 

⁻⁵

=



_{ie (12)}

Postać antysymetryczną 𝛀 wektora 𝛚 wyznacza się z równania:

 

 







 











−



−







−

=





=

0 0 0

1 2

1 3

2 3

Ω

₍₁₃₎

Postać antysymetryczną Ω

𝑒𝑛𝑛

wektora prędkości kątowej układu nawigacyjnego względem ziemskiego (ang.

transport rate) 𝛚

_𝑒𝑛^𝑛

można wyliczyć ze wzoru:

( ) ( ) ( ) ( ) ^{ }

 

 







 

 

 







+



− 

+

− 

+



=

h R

v

h R

v h R

v

E n

E es

N n

N es E

n E es

n en

,

tg

1 , 1 , 1

ω

₍₁₄₎

gdzie 𝑅

_𝑁

i 𝑅

_𝐸

to promienie południkowy i równoleżnikowy elipsoidy ziemskiej, v

_{𝑒𝑠1,𝑁}^𝑛

i v

_{𝑒𝑠1,𝐸}^𝑛

stanowią składowe wektora v

_𝑒𝑠1^𝑛

prędkości układu s-frame względem układu e-frame, wyrażonego w układzie n-frame, zaś ℎ to wysokość lotu nad elipsoidą odniesienia. Promień południkowy elipsoidy ziemskiej, według modelu WGS-84, można wyznaczyć z zależności:

( ) (

^{2 2}

( ) )

²³

2 0

sin 1

1



−

= − e

e R

_N

R

(15)

gdzie 𝑅

₀

to promień równikowy elipsoidy WGS-84 (𝑅

₀

= 6378137 m), 𝑒 zaś stanowi mimośród elipsoidy (𝑒 = 0,081819108425). Promień równoleżnikowy elipsoidy WGS-84, w punkcie o zadanej szerokości geograficznej, określa wzór:

( ) ^

= −

2 2

0

sin 1 e

R

_E

R

₍₁₆₎

5.2. Aktualizacja prędkości

Celem obliczeń etapu aktualizacji prędkości realizowanych w INS jest wyznaczenie wektora v

_𝑒𝑠1^𝑛

prędkości obiektu (układu s-frame) względem Ziemi (układu e-frame), wyrażonego w układzie n-frame. Dane

7

wejściowe toru obliczeniowego stanowi wektor przyspieszenia pozornego f

_𝑖𝑠1^𝑠1

, mierzony przez czujnik IMU z częstotliwością 1 kHz.

W pierwszym kroku wektor przyspieszenia pozornego przeliczany jest z układu s1-frame do b-frame:

1 , 1 1 ,

1

s k is b s b

k

is

C f

f =

₍₁₇₎

Następnie wektor ten przeliczany jest do układu n-frame z wykorzystaniem macierzy C

_𝑏^𝑛

, uprzednio wyznaczonej w etapie aktualizacji orientacji przestrzennej:

b k is n

k b n

k

is1,

C

,

f

1,

f =

₍₁₈₎

Przyrost prędkości υ

_𝑖𝑠1^𝑛

, powstający w wyniku występowania przyspieszenia pozornego obiektu, wyznaczany jest poprzez całkowanie przyspieszenia metodą trapezów:

(

isⁿ k

)

^t

n k is n

k

is_1, = _1, + _{1, −1} 

2

1 f f

υ ₍₁₉₎

gdzie Δ𝑡 = 𝑡

_𝑘

− 𝑡

_𝑘−1

. Korygując przyrost prędkości wyznaczony w (19) o składniki związane z przyspieszeniem wynikającym z siły ciężkości g

_𝑏^𝑛

otrzymuje się następującą zależność:

n

t

k b n

k is n

k es n

k

es_1,

= v

_1,−1

+ υ

_,

+ g

_{, −1}



v

₍₂₀₎

gdzie g

_𝑏^𝑛

to wektor przyspieszenia ziemskiego wyrażony w układzie n-frame. W powyższym równaniu pominięto wpływ ruchu układu n-frame względem układu e-frame (transport rate) oraz składnik związany z efektem Coriolisa.

5.3. Aktualizacja położenia

Celem obliczeń etapu aktualizacji położenia jest określenie wektora r

_𝑠1^𝑒

położenia początku układu s1-frame, wyrażonego w układzie e-frame:



_{s s s}



^T

e

s₁

= 

₁



₁

h

₁

r

(21)

W badanym systemie nawigacyjnym czujnik IMU znajduje się w pobliżu środka ciężkości obiektu (początku układu b-frame), dlatego przyjęto założenie, że w układzie e-frame współrzędne położenia początków tych układów (punkty 𝑂

^𝑠1

i 𝑂

^𝑏

) są takie same, tzn.:

e s e b

r

1

r =

(22)

Zakładając dodatkowo, że w trakcie jednego okresu pomiarowego zmiana prędkości jest liniowa, całkowanie numeryczne można wykonać metodą trapezów:

(

_esⁿ _{Dk es}ⁿ _Dk

)

k s k s k

b

t v v

h h

h

_{, 1, 1, 1 1, , 1 1, ,}

2 +

− 

=

=

_{− − (23)}

 





 





+ + +

+ 



=



=



−

−

−

−

−

k s k N

n k N es k

s k N

n k N es k

s k s k

b

R h

v h

R t v

, 1 1 ,

, , 1

1 , 1 1 ,

1 , , 1 1

, 1 , 1

,

2

⁽²⁴⁾

( ) ( ) ( ) ( ) ^{ } _ ^

 





 + +

 +

+ 



=



=



−

−

−

−

−

k s k

s k E

n k E es k

s k

s k E

n k E es k

s k s k

b

R h

v h

R t v

, 1 ,

1 ,

, , 1

1 , 1 1

, 1 1 ,

1 , , 1 1

, 1 , 1

,

2 cos cos

⁽²⁵⁾

8 6. Instrukcja ćwiczenia laboratoryjnego

Celem ćwiczenia jest zapoznanie z algorytmem oraz obliczeniami realizowanymi w systemie nawigacji inercjalnej. Zadanie Studenta polega na przeanalizowaniu kodu źródłowego napisanego w języku MATLAB oraz uzupełnienie jego brakujących fragmentów na podstawie równań przedstawionych w powyższej części niniejszej instrukcji.

6.1. Przeanalizuj kod programu: zidentyfikuj główne etapy obliczeń (zgodnie z rys. 3) oraz miejsca w których konieczne będzie uzupełnienie kodu (wskazówka – szukaj miejsc z napisem UZUPELNIJ).

6.2. Uzupełnij brakujące fragmenty programu.

6.3. Przygotuj sprawozdanie z ćwiczenia laboratoryjnego, a w nim:

a) Opisz sposób wyznaczenia orientacji układu s-frame względem b-frame (kąty Φ

_𝑏𝑠1

, Θ

_𝑏𝑠1

, Ψ

_𝑏𝑠1

).

b) Zinterpretuj wyznaczoną macierz C

_𝑠1^𝑏

.

c) Opisz sposób przeliczania wektora przyspieszenia pozornego z układu s-frame do b-frame oraz zinterpretuj uzyskane przebiegi

d) Uzasadnij wartości przyjęte podczas inicjalizacji orientacji przestrzennej, prędkości i położenia obiektu.

e) Zinterpretuj uzyskane wyniki wyznaczania kątów Eulera, prędkości oraz położenia obiektu.

f) W funkcji odpowiedzialnej za aktualizację orientacji przestrzennej usuń składniki związane

z transport rate oraz earth rate oraz porównaj uzyskane wyniki wyznaczania położenia

z rezultatami, w których te czynniki korekcyjne są uwzględniane.

9

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 błędów INS

Warszawa, 2017

11 1. Cel ćwiczenia

Celem ćwiczenia jest modelowanie, implementacja i symulacja komputerowa działania pojedynczego kanału systemu nawigacji inercyjnej.

2. Założenia

Analizowane będą błędy pojedynczego kanału horyzontalnego systemu INS. Model zawiera wiele uproszczeń w stosunku do rzeczywistego systemu INS. Uproszczenia opisano poniżej.

Nawigacja (określanie prędkości i położenia pojazdu) odbywa się w lokalnym horyzontalnym układzie współrzędnych, a więc w układzie obracającym się względem przestrzeni inercjalnej ze względu na ruch pojazdu i obrót Ziemi. W dalszych rozważaniach ruch obrotowy Ziemi pominięto.

Ruch pojazdu odbywa się wyłącznie w jednej płaszczyźnie, a jego położenie jest opisywane za pomocą dwóch współrzędnych x i z. Schemat INS nawigującego w pojedynczej płaszczyźnie, w obracającym się układzie współrzędnych, jest następujący:

Rys. 1. Uproszczony dwuwymiarowy INS nawigujący w obracającym się układzie współrzędnych

Równania nawigacji inercjalnej systemu INS nawigującego w pojedynczej płaszczyźnie, w obracającym się układzie współrzędnych, są następujące:

z R

v

_x

yb

− +



=



0



₍₁₎

 +



= f cos f sin

f

_{x xb zb} (2)

 +



−

= f sin f cos

f

_{z xb zb} (3)

12

z

z x

x

= + +

0 x

R v f v

v

(4)

z

z

z

= + − +

0 2 x

R g v f

v

(5)

v

x

x =  (6)

v

z

z =  (7)

Pojazd porusza się ruchem prostoliniowym jednostajnym lub pozostaje w spoczynku, dzięki czemu można założyć, że podlega on wyłącznie oddziaływaniom grawitacyjnym. Przyspieszeniomierze systemu INS mierzą zatem wyłącznie przyspieszenia pozorne niezbędne do pokonania grawitacji, a zatem: f

xg

= 0 i f

zg

= g. Dla uproszczenia przyjęto kąt pochylenia  = 0. Na podstawie poprzedniego rysunku i równań, można podać schemat i równania przedstawiające propagację błędów:

Rys. 2. Schemat propagacji błędów w obracającym się układzie współrzędnych

Rys. 3. Równania propagacji błędów w obracającym się układzie współrzędnych

13

Na podstawie analizy równań propagacji można wyprowadzić następujące zależności opisujące wpływ poszczególnych błędów INS na błąd położenia:

Rys. 4. Wpływ poszczególnych błędów INS na błąd wyznaczania położenia

3. Zadania do wykonania

Utworzyć w Simulinku model symulujący propagację błędów uproszczonego pojedynczego kanału INS zgodnie z opisanymi uprzednio równaniami i następującym rysunkiem:

Rys. 5. Model symulacyjny błędów uproszczonego pojedynczego (horyzontalnego) kanału INS

Przyjąć następujące założenia i sposób postępowania:

• Czas symulacji 4 h.

• Początkowe wartości wszystkich błędów zerowe.

14

• Przeprowadzić wstępną symulację w celu sprawdzenia działania modelu.

Następnie:

• Wprowadzić początkowy błąd położenia  x

0

= 1000 m. Sprawdzić wpływ tego błędu na przebieg błędów położenia  x, prędkości  v i pochylenia  .

• Wprowadzić początkowy błąd prędkości  v

0

= 1 m/s. Sprawdzić wpływ tego błędu na przebieg błędów położenia  x, prędkości  v i pochylenia  . Określić na podstawie wyników symulacji amplitudę i okres oscylacji błędów położenia. Porównać uzyskane wyniki z obliczonymi w sposób analityczny (zależności z rys. 4).

• Wprowadzić początkowy błąd pochylenia 

0

= 0,1. Sprawdzić wpływ tego błędu na przebieg błędów położenia  x, prędkości  v i pochylenia  . Określić na podstawie wyników symulacji amplitudę i okres oscylacji błędów położenia. Porównać uzyskane wyniki z obliczonymi w sposób analityczny (zależności z rys. 4).

• Wprowadzić błąd przyspieszeniomierza (błąd systematyczny)  f

xb

= 100 g (typowe dla przyspieszeniomierzy MEMS), a następnie  f

xb

= 1 g (typowe dla przyspieszeniomierzy mechanicznych). Sprawdzić wpływ tego błędu na przebieg błędów położenia  x, prędkości  v i pochylenia  . Określić na podstawie wyników symulacji amplitudę i okres oscylacji błędów położenia. Porównać uzyskane wyniki z obliczonymi w sposób analityczny (zależności z rys. 4).

• Wprowadzić błąd giroskopu (błąd systematyczny) 

yb

= 100/h (typowe dla giroskopów MEMS), a

następnie 

yb

= 0,01/h (typowe dla giroskopów FOG i RLG). Sprawdzić wpływ tego błędu na

przebieg błędów położenia  x, prędkości  v i pochylenia  . Określić na podstawie wyników

symulacji amplitudę i okres oscylacji błędów położenia. Porównać uzyskane wyniki z obliczonymi

w sposób analityczny (zależności z rys. 4).

15

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 pt.:

Analiza działania systemu INS/GPS

Warszawa, 2018

17 1. Cel ćwiczenia

Celem ćwiczenia jest analiza systemu zintegrowanego INS/GPS. W ramach ćwiczenia należy:

• dokonać analizy działania systemu INS/GPS

• zaimplementować równania filtru Kalmana w oprogramowaniu Matlab,

• dokonać analizy uzyskanych rezultatów wyznaczania położenia.

2. Opis systemu nawigacyjnego

Modelowany zintegrowany system nawigacyjny składa się z systemu nawigacji inercjalnej INS oraz odbiornika GPS (rys.2.1). W systemie tym intergracja danych realizowana jest metodą filtracji pośredniej z korekcją w przód.

Rys.2.1. Model systemu INS/GPS

a. Model błędów INS

W prezentowanym systemie modelowane są błędy wyznaczania położenia w płaszczyźnie horyzontalnej, tzn. wzdłuż osi północnej:

 



 





+



−

=



+



=





=



E N E

BN E N

N

u v

u v

v N

R 1 g







(1)

oraz osi wschodniej:

 



 





+



=



+



−

=





=



N E N

BE N E

E

u v

u v

v E

R 1 g







(2)

gdzie:

 N - błąd wyznaczenia położenia przez INS w osi północnej, v

N

 - błąd wyznaczenia prędkości przez INS w osi północnej,



E

- błąd wyznaczenia orientacji kątowej b-frame względem n-frame, wyrażony wokół osi wschodniej,

Uncorrected INS errors

GPS errors

Corrected INS errors

Estimated INS errors

Comparison of North errors Comparison of East errors

--- Note:

Doub le-click b locks to change parameters or configure sub systems.

z(1) z(2)

x(1) x(4) Kalman Filter dN

dE INS

[dE_GPS]

[dN_GPS]

[dE_INS]

[dN_INS]

[dE_INSGPS]

[dN_INSGPS]

dN dE GPS

[dE_GPS]

[dE_INS]

[dE_INSGPS]

[dN_GPS]

[dN_INS]

[dN_INSGPS]

18

 E - błąd wyznaczenia położenia przez INS w osi wschodniej, v

E

 - błąd wyznaczenia położenia przez INS w osi wschodniej,



N

- błąd wyznaczenia orientacji kątowej b-frame względem n-frame, wyrażony wokół osi północnej, g – przyspieszenie ziemskie,

R – promień Ziemi (przy założeniu modelu kulistego, R = 6371 km).

Równania (1) i (2) zaimplementowano w środowisku Simulink (rys. 2.2 – 2.4). Parametrami wejściwymi modelu błędów są widmowe gęstości mocy szumu przyspieszeniomierzy (SBN, SBE), widmowe gęstości mocy szumu giroskopów (SWN, SWE) oraz błędy orientacji osi pomiarowych czujników. Szumy modelowane są jako ograniczone pasmowo szumy białe.

Rys. 2.2. Model błędów INS

Rys. 2.3. Model błędów INS - kanał N

19

Rys. 2.3. Model błędów INS - kanał E

b. Model błędów odbiornika GPS

Błędy wyznaczania położenia wzdłuż osi północnej i wschodniej przez odbiornik GPS modelowane są jako szumy o rozkładzie normalnym, z zerową wartością oczekiwaną. W modelu możliwe jest także ustawienie wartości wariancji ww. błędów.

Rys. 2.3. Model błędów odbiornika GPS

c. Ciągły model dynamiki systemu

Ciągły model dynamiki systemu dany jest zależnością:

( ) t Fx ( ) t Gu ( ) t

x  = +

(3)

gdzie:

G - macierz sterowań stochastycznych, F - macierz podstawowa (przejścia),

( ) t

x - n-wymiarowy wektor stanu systemu w chwili t,

20

( ) t

u - p-wymiarowy wektor wymuszeń stochastycznych (zakłóceń procesu).

Wektor stanu zawiera błędy wyznaczania położenia, prędkości i orientacji kątowej przez system INS, tzn.:

( )

 

 

 







 

 

 











 





=

N E E

N

v E v

N

t

x

₍₄₎

Uwzględniając zależności (1)-(4) otrzymuje się:

 

 





 

 







 

 

 







 

 

 







+

 

 

 







 

 

 











 







 

 

 

 







 

 

 

 







−

= −

 

 

 







 

 

 























N BE E BN

N E E

N

N E E

N

u u u u

v E v

N

v E v

N

1 0

0 1

0 0

0 0

0 0

0 0

0 0

0 0

0 0

1 0

0 1

0 0

R 0 0 1

g 0 0

0 1 0

0 0 0

0 0 0

0 0 0

0 0 0

0 0 0

0 0 0

R 0 0 1

g 0 0

0 1 0













(5)

Macierz kowariancji zakłóceń procesu:

 

 





 

 





=





N BE E BN

S 0

0 S 0 0

0 0

0 0

0 0 S

0 0 S

Q

c ₍₆₎

Ciągły model systemu (3) często wygodnie jest zastąpić modelem dyskretnym, ponieważ dane z wielu urządzeń nawigacyjnych są dostępne jedynie w dyskretnych chwilach czasu, a do ich przetwarzania w systemach zintegrowanych zwykle stosuje się urządzenia mikroprocesorowe, realizujące dyskretne algorytmy filtracji. Dyskretny, liniowy model dynamiki dany jest zależnością:

( k ) x ( ) ( ) k w k

x + 1 =  +

(7)

Gdzie Φ to macierz tranzycyjna (przejścia) związana z macierzą podstawową poprzez zależność:

( ) ( ) ( )

... !

! 2

2

n T T T

e k

n

T

F F

F

Φ =

^F

= I + + + +

(8)

Dyskretny model dynamiki systemu INS/GPS przyjmuje postać:

21

( )

( )

( )

( )

( )

( )

( )

( ) ( )

( ) ( ) ( ) ( )

( ) ( )

( ) ( ) ( ) ( ) ^{ }

 

 







 

 

 







+

 

 

 







 

 

 











 







=

 

 

 







 

 

 







+

 +

 +

  + +

 +















k w

k w

k w

k w

k w

k w

k k v

k E

k k v

k N

k

k k v

k E

k k v

k N

N vE

E E vN

N

N E E

N

N E E

N

Φ

1 1 1 1 1 1

(9)

d. Model obserwacji

Dyskretny model obserwacji danych jest następującym równaniem:

( ) k H ( ) ( ) ( ) k x k v k

z =  +

(10)

gdzie:

( ) k

H - macierz obserwacji dyskretnego modelu systemu w chwili kT,

( ) k

v - m-wymiarowy wektor błędów pomiarowych w chwili kT,

( ) ^k

z - m-wymiarowy wektor pomiarowy w chwili kT.

W omawianym systemie INS/GPS wektor pomiarowy z(k) ma postać:

( ) _



 







−





−

= 

GPS INS

GPS INS

E E

N k N

z

₍₁₁₎

gdzie:  N

_INS

i  E

_INS

to błędy wyznaczania położenia z modelu sytemu INS, natomiast  N

_GPS

i  E

_GPS

to błędy wyznaczania położenia z modelu odbiornika GPS.

Macierz kowariancji błędów pomiarowych R ( ) k określana jest dla wariancji danych pochodzących z odbiornika GPS i dana jest zależnością:

  



 







= 

=

² ₂

0 0

E N

E vv

T

R

₍₁₂₎

3. Liniowy filtr Kalmana

W przentowanym systemie integracja danych INS i GPS realizowana jest za pomocą filtru Kalmana.

Ponieważ zarówno model dynamiki jak i model obserwacji są liniowe możliwe jest zastosowanie klasycznego, liniowego filtru Kalmana, który w omawianym przypadku jest algorytmem filtracji optymalnej.

Obliczenia realizowane w filtrze przedstawiono na rys. 3.1.

22

Rys. 3.1. Algorytm liniowego filtru Kalmana

Zmienne wykorzystane na rys. 3.1:

I - macierz jednostkowa,

( ) k k

xˆ - estymata wektora stanu w chwili kT – wynik filtracji,

( ) k k

P - macierz kowariancji błędów filtracji w chwili kT,

( ^{k 1 k} )

ˆ +

x - estymata wektora stanu w chwili (k+1)T – wynik predykcji,

( k 1 + k )

P - macierz kowariancji błędów predykcji w chwili (k+1)T,

( ) k

K - macierz wzmocnień Kalmana w chwili kT.

Kluczowe operacje wykonywane podczas pracy filtru to: inicjalizacja (1), predykcja wektora stanu i macierzy kowariancji błędów (2), wykonywanie pomiaru (3), obliczanie macierzy wzmocnień Kalmana i korekcja wyników predykcji w wykorzystaniem ostatnio wykonanego pomiaru (4).

4. Instrukcja ćwiczenia laboraatoryjnego Zadania do wykonania:

a. Zaimplementować równania filtru Kalmana (w funkcji z pliku mdlUpdate() Lab1_KF.m),

b. Wyznaczyć wartości elementów macierzy tranzycyjnej przy ograniczeniu rozwinięcia w szereg Fouriera do wyrazu związanego z wielomianem stopnia pierwszego,

c. Zinterpretować wyniki wyznaczania położenia (INS, GPS, INS/GPS w kanale północnym i wschodnim). Parametry symulacji:

Filtr Kalmana: PSD przyspieszeniomierzy 2𝑒 − 5

^𝑚2

𝑠³

, PSD giroskopów 1𝑒 − 8

^{𝑟𝑎𝑑2}

𝑠

, odchylenie standardowe błędów GPS 10 m,

INS: PSD przyspieszeniomierzy 2𝑒 − 5

^𝑚2

𝑠³

, PSD giroskopów 1𝑒 − 8

^{𝑟𝑎𝑑2}

𝑠

, GPS: wartość oczekiwana 0 m, wariancja błędów GPS 100 m,

d. Zinterpretować wyniki wyznaczania położenia (INS, GPS, INS/GPS w kanale północnym

i wschodnim). Parametry symulacji:

23 Filtr Kalmana: PSD przyspieszeniomierzy 2𝑒 − 6

^𝑚2

𝑠³

, PSD giroskopów 1𝑒 − 9

^{𝑟𝑎𝑑2}

𝑠

, odchylenie standardowe błędów GPS 10 m,

INS: PSD przyspieszeniomierzy 2𝑒 − 5

^𝑚2

𝑠³

, PSD giroskopów 1𝑒 − 8

^{𝑟𝑎𝑑2}

𝑠

, GPS: wartość oczekiwana 0 m, wariancja błędów GPS 100 m,

e. Wyznaczyć wartości elementów macierzy tranzycyjnej przy ograniczeniu rozwinięcia w szereg Fouriera do wyrazu związanego z wielomianem stopnia drugiego,

f. Zinterpretować wyniki wyznaczania położenia (INS, GPS, INS/GPS w kanale północnym i wschodnim). Parametry symulacji:

Filtr Kalmana: PSD przyspieszeniomierzy 2𝑒 − 5

^𝑚2

𝑠³

, PSD giroskopów 1𝑒 − 8

^{𝑟𝑎𝑑2}

𝑠

, Odchylenie standardowe błędów GPS 10 m,

INS: PSD przyspieszeniomierzy 2𝑒 − 5

^𝑚_𝑠3²

, PSD giroskopów 1𝑒 − 8

^𝑟𝑎𝑑_𝑠²

,

GPS: wartość oczekiwana 0 m, wariancja błędów GPS 100 m,

24

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 nawigacji zliczeniowej

Warszawa, 2017

25 5. Cel ćwiczenia

Celem ćwiczenia jest analiza systemu nawigacji zliczeniowej wykorzystującego odometrię różnicową.

W ramach ćwiczenia należy dokonać pomiaru odpowiednich wymiarów geometrycznych robota kołowego, zaimplementować równania nawigacyjne w programie wykonywanym w sterowniku robota (wykorzystując język C oraz środowisko RobotC) oraz ocenić dokładność wyznaczania położenia i kursu.

6. Opis platformy pomiarowej

Platformą pomiarową wykorzystywaną w ćwiczeniu jest robot zbudowany w oparciu o zestaw LEGO Mindstorm EV3. Posiada on jedną oś napędową, której każde z kół wyposażone jest silnik DC z enkoderem o rozdzielczości 360 impulsów na jeden obrót koła. W części tylnej robota znajduje się metalowa kulka podporowa zapewniająca stabilność platformy. Prędkość obrotowa robota wokół osi pionowej mierzona jest przez giroskop, zaś jej całkowanie pozwala na wyznaczenie kursu niezależnie od sygnałów z enkoderów.

Sterowanie kierunkiem i prędkością jazdy robota odbywa się za pomocą pilota przesyłającego dane do robota w paśmie podczerwieni. Fotografię robota przedstawiono na rys. 1

Rys. 1. Platforma kołowa wykorzystywana podczas badań

Z platformą pomiarową związany jest układ b-frame, w którym oś OX

^b

skierowana jest w kierunku przedniej części pojadu, oś OY

^b

skierowana jest w jego prawą stronę, natomiast oś OZ

^b

dopełnia układ prawoskrętny, co przedstawiono na rys. 2. Na poniższym rysunku przez b oznaczono odległość pomiędzy kołami pojazdu, mierzoną wzdłuż osi OY

^b

, natomiast D stanowi średnicę koła.

Rys. 2. Schemat platformy kołowej

7. Odometria różnicowa

Położenie i kurs ruchu robota wyznaczane są w lokalnym układzie odniesienia oznaczonym jako n-frame.

W chwili początkowej osie układu n-frame (OX

ⁿ

, OY

ⁿ

, OZ

ⁿ

) pokrywają się z osiami układu b-frame

związanego z robotem. Podczas ruchu robota układ nawigacyjny zachowuje jednak stałe położenie

26

i orientację względem Ziemi, natomiast zmianie ulega położenie i orientacja układu b-frame, co przedstawiono na rys. 3

Rys. 3. Ruch robota w układzie n-frame

Zmiana położenia prawego koła w jednym okresie pomiarowym wynosi:

1

D

,

,

   

 



 −

=



⁻

m n

d

_R

n

^{Rk Rk} ₍₁₎

gdzie  d

_R

to zmiana położenia prawego koła pomiędzy pomiarem k-1 oraz k, n

^R

to liczba impulsów z enkodera prawego koła, natomiast to m rozdzielczość enkodera. Zależność opisująca zmianę położenia lewego koła jest analogiczna:

1

D

,

,

   

 



 −

=



⁻

m n

d

_L

n

^{Lk Lk} (2)

Zmiana położenia punktu O

^b

dana jest wzorem:

2

R

L

d

d =  d + 



₍₃₎

Dzięki zastosowaniu odometrów na obu kołach robota możliwe jest wyznaczenie kursu jego ruchu względem układu n-frame. W jednym okresie pomiarowym przyrost kursu  wyznaczany jest z równania:

b

R

L

d

d − 

= 



(4)

Położenie robota oraz jego kurs w chwili k wyznaczane są poprzez sumowanie przyrostów tych wielkości z

chwil poprzednich, zgodnie z zależnościami:

27

(  +  )

 +

=

_k−1

cos

_k−1

k

x d

x

₍₅₎

( ^{ + } )

 +

=

_k−1

sin

_k−1

k

y d

y

₍₆₎



+



=



_{k k−1 (7)}

8. Instrukcja laboratoryjna

Ćwiczenie laboratoryjne składa się z dwóch części: programistycznej i doświadczalnej.

W części pierwszej należy zaimplementować rówanania nawigacyjne w oprogramowaniu robota (w programie szukaj frazy „UZUPEŁNIJ”).

W części drugiej należy sprawdzić dokładność wyznaczania położenia i kursu obliczanych w ramach odometrii różnicowej:

a) wykonaj przejazd po linii prostej, na jak najdłuższym odcinku, porównaj wynik odometrii z wartością określoną za pomocą taśmy mierniczej; wnioski,

b) w miarę możliwości wykonaj obrót o 90º, 180º, 270º oraz 360º wokół osi OZ

^b

, po każdym obrocie zapisz wynik odometrii i wyznaczony za pomocą giroskopu oraz zmierz rzeczywisty kąt obrotu;

wnioski,

c) w miarę możliwości wykonaj obrót o -90º, -180º, -270º oraz -360º wokół osi OZ

^b

, po każdym obrocie zapisz wynik odometrii i wyznaczony za pomocą giroskopu oraz zmierz rzeczywisty kąt obrotu;

wnioski,

d) wykonaj 10 pełnych obrotów wokół osi OZ

^b

starając zatrzymać się w pozycji zbliżonej do początkowej, zapisz wynik odometrii i wartość kursu wyznaczoną za pomocą giroskopu; wnioski, e) wykonaj przejazd po trasie z rys. 4, w każdym z narożników zapisz wyniki odometrii i wyznaczone za

pomocą giroskopu oraz porównaj je z wartościami zmierzonymi linijką i kątomierzem; wnioski.

Rys. 4. Trasa robota podczas badania

28

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, 2018

29 1. Cel ćwiczenia

Celem ćwiczenia jest analiza systemu nawigacji zliczeniowej wykorzystującego giroskop jednoosiowy.

W ramach ćwiczenia należy zaimplementować wybrane metody całkowania numerycznego (wykorzystując język C oraz środowisko RobotC) oraz ocenić dokładność wyznaczania kursu.

2. Opis systemu pomiarowego

Platformą pomiarową wykorzystywaną w ćwiczeniu jest robot zbudowany w oparciu o zestaw LEGO Mindstorm EV3. Prędkość obrotowa robota wokół osi pionowej mierzona jest przez giroskop jednoosiowy.

Sterowanie kierunkiem i prędkością jazdy robota odbywa się za pomocą pilota przesyłającego dane do robota w paśmie podczerwieni. Fotografię robota przedstawiono na rys. 1

Rys. 1. Platforma kołowa wykorzystywana podczas badań

3. Metody całkowania numerycznego

Wartość kursu, czyli kąta mierzonego pomiędzy kierunkiem odniesienia a osią podłużną robota, 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

₍₁₎

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 ,

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

₀

b ,

c) środkowych prostokątów gdy x

₀

= ( b − a ) / 2 .