metody korekcji kątów obrotu heading, pitch, roll z użyciem

MODELOWANIE INŻYNIERSKIE 2016 nr 58, ISSN 1896-771X
METODY KOREKCJI KĄTÓW OBROTU
HEADING, PITCH, ROLL
Z UŻYCIEM BEZZAŁOGOWEGO
STATKU POWIETRZNEGO
Damian Wierzbicki1a, Kamil Krasuski2b
Zakład Fotogrametrii i Teledetekcji, Wydział Inżynierii Lądowej i Geodezji, Wojskowa Akademia Techniczna
2
Wydział Geodezji, Kartografii i Katastru Nieruchomości, Starostwo Powiatowe w Rykach
a
[email protected], b [email protected]
1
Streszczenie
W pracy przedstawiono rezultaty korekcji kątów HPR (Heading, Pitch i Roll) z użyciem filtracji Kalmana, metody wielomianowej oraz metody trygonometrycznej. Eksperyment badawczy przeprowadzono z użyciem surowych
wartości kątów Heading, Pitch i Roll, zarejestrowanych przez urządzenie Trimble UX-5. W artykule przedstawiono algorytmy korekcji kątów HPR oraz opisano konfigurację parametrów wejściowych dla każdej metody badawczej. Kod źródłowy programu i obliczenia numeryczne zostały wykonane w edytorze Scilab 5.4.1.
Słowa kluczowe: Heading, Pitch, Roll, BSP, odchylenie standardowe, filtr Kalmana, metoda wielomianowa, metoda
trygonometryczna
THE CORRECTION METHODS OF HEADING, PITCH,
ROLL ROTATION ANGLES WITH USING UAV
Summary
In the paper, results of correction Heading, Pitch and Roll angles with using Kalman filtering method, polynomial
method and trigonometric method were presented. The research test was realized using the raw data of Heading,
Pitch and Roll angles, which are register by Trimble UX-5 platform. In the paper, algorithms of correction Heading, Pitch and Roll angles were presented and configuration of initial parameters in each research method was described. The source code of program and numerical computations were executed in Scilab 5.4.1 software.
Keywords: Heading, Pitch, Roll, UAV, standard deviation, Kalman Filter, polynomial method, trigonometric
method
1.
WSTĘP
Orientacja w przestrzeni Bezzałogowego Statku Powietrznego (BSP) odbywa się zazwyczaj z wykorzystaniem połączenia sensora GPS oraz systemu inercjalnego
INS. Sensor GPS pozwala na wyznaczenie współrzędnych BSP względem środka Ziemi w układzie ECEF,
natomiast sensor INS umożliwia określenie przyspieszenia i kątów obrotu HPR (Heading, Pitch, Roll) w układzie wewnętrznym statku powietrznego (tzw. „body
frame”) [1, 7]. Kąty HPR przyjęto definiować w polskiej
nomenklaturze następująco: Heading- kurs, Pitch- kąt
pochylenia, Roll- kąt obrotu [8]. Należy podkreślić, iż
zarejestrowane przez jednostkę inercjalną IMU wartości
kątów HPR mogą zawierać błędy grube i powinny
zostać poddane dodatkowej obróbce wewnętrznej w celu
detekcji i eliminacji pomiarów odstających. Nieprecyzyjne wyznaczone wartości kątów HPR przekładają się
głównie na stabilność parametrów lotu i położenia
platformy BSP [6]. Dopuszczalna dokładność określenia
132
DAMIAN WIERZBICKI, KAMIL KRASUSKI
- dla kąta Roll:
kątów HPR dla urządzenia BSP, podczas wykonywania
lotu, może wynosić nawet do 20. Trzeba nadmienić, iż
większość producentów BSP oferuje możliwość zapisu
wartości kątów HPR dla BSP z precyzją do 2 miejsc po
przecinku (tj. 0.010), co nie jest tożsame z uzyskiwaną
dokładnością bezwzględną odczytu [4]. Zarejestrowane
kąty HPR po wstępnej obróbce danych źródłowych są
wykorzystywane w obszarze fotogrametrii do określenia
elementów orientacji zewnętrznej dla pozyskanych zdjęć
lotniczych z niskiego lub średniego pułapu wysokości [3].
φ (k ) = φ (k − 1) + wφ (k − 1) + ( dφ (k − 1) + wd φ (k − 1) ) ⋅ ∆t
(3)

dφ (k ) = dφ (k − 1) + wdφ (k − 1)

gdzie:
[ψ (k ),θ (k ),φ (k )] -
skorygowane wartości kątów HPR na
epokę k (epoka bieżąca),
[ψ (k − 1),θ (k − 1),φ (k − 1)] -
skorygowane wartości kątów
HPR na epokę k-1 (epoka poprzednia),
W prezentowanej pracy przedstawiono i omówiono
rezultaty korekcji danych HPR z użyciem filtru Kalmana, metody wielomianowej i trygonometrycznej. Całość
artykułu podzielono na pięć części: wstęp, metodologia
badań, opis eksperymentu badawczego, wyniki i dyskusja, wnioski końcowe. Algorytmy i modele matematyczne
wykorzystane w korekcji danych źródłowych w postaci
kątów HPR zostały opisane szczegółowo w rozdziale
drugim. W rozdziale trzecim scharakteryzowano eksperyment badawczy i opisano konfigurację parametrów
wejściowych w każdej metodzie badawczej. W rozdziale
czwartym zaprezentowano uzyskane rezultaty z eksperymentu badawczego oraz dokonano ich porównania z
surowymi odczytami kątów HPR z urządzenia Trimble
UX-5. Artykuł naukowy kończy rozdział z wnioskami
oraz spis literatury.
 wψ ( k − 1), wθ ( k − 1), wφ (k − 1)  - szum procesu pomiarowego dla kątów HPR na epokę k-1 (epoka poprzednia),
[ dψ (k − 1), dθ (k − 1), dφ (k − 1)] -
dryfty kątów HPR na
epokę k-1 (epoka poprzednia),
 wdψ (k − 1), wdθ (k − 1), wd φ (k − 1)  - szum procesu pomiarowego dla dryftów kątów HPR na epokę k-1 (epoka
poprzednia),
∆t - przyrost czasu pomiędzy epokami (k) oraz (k-1),
[ dψ (k ), dθ (k ), dφ (k )] -
dryfty kątów HPR na epokę k
(epoka bieżąca).
Równania (1), (2) i (3) są ogólnymi równaniami
modelu systemu pomiarowego, w którym dane wejściowe pochodzą tylko z jednego urządzenia pomiarowego, np. żyroskop laserowy. W przypadkach
szczegółowych, gdy system pomiarowy składa się z
kilku rządzeń mierniczych (np. akcelerometry, żyroskopy, inklinometry), należy zastosować trójwymiarowy model systemu, tzn. uwzględnić parametr
prędkości kątowej. W równaniach (1), (2) i (3) zastosowano dwuwymiarowy model systemu pomiarowego, który zawiera informacje o wyznaczanych
kątach HPR oraz ich dryfcie. Wartości kątów HPR
są wyrażone w stopniach lub radianach, zaś dryfty
kątów HPR odpowiednio w stopniach na sekundę
lub radianach na sekundę. Równania systemu pomiarowego (1), (2) i (3) są wyznaczane z użyciem
filtru Kalmana w procesie dwuetapowym, jak poniżej [11]:
2. METODYKA BADAŃ
W metodologii badań zaproponowano użycie trzech
modeli matematycznych, mających na celu poprawę
wartości kątów obrotu HPR. W analizie wykorzystano
metodę filtracji Kalmana (rozwiązanie 1), metodę wielomianową (rozwiązanie 2), metodę trygonometryczną
(rozwiązanie 3). Model matematyczny dla każdej z wyżej
wymienionych metod badawczych został szczegółowo
opisany w niniejszym artykule.
2.1 FILTR KALMANA
W pierwszej metodzie badawczej zastosowano algorytm
dwuwymiarowego modelu systemu pomiarowego z
użyciem filtracji Kalmana w przód. Parametrami wejściowymi dla algorytmu są wartości kątów HPR (Heading, Pitch, Roll), zarejestrowane przez BSP dla określonego interwału czasu. Podstawowe równanie modelu
systemu pomiarowego dla kątów HPR przyjmie postać
[2]:
1) I etap- proces „predykcji”:
x(k − 1) = A ⋅ x(k − 1)
P(k − 1) = A ⋅ P( k − 1) ⋅ AT + Q(k − 1)
- dla kąta Heading:
ψ ( k ) = ψ ( k − 1) + wψ ( k − 1) + ( dψ ( k − 1) + wdψ ( k − 1) ) ⋅ ∆t
(1)

dψ ( k ) = dψ ( k − 1) + wdψ ( k − 1)

gdzie:
A - macierz współczynników,
1 ∆t 
A=
,
0 1 
- dla kąta Pitch:
θ (k ) = θ (k − 1) + wθ (k − 1) + ( dθ (k − 1) + wdθ (k − 1) ) ⋅ ∆t
(2)

dθ (k ) = dθ (k − 1) + wdθ (k − 1)

133
(4)
(5)
METODY KOREKCJI KĄTÓW OBROTU HEADING, PITCH, ROLL (...)
W zagadnieniu ogólnym, stosując metodę wielomianową,
sprowadza się ją do wyznaczenia współczynników liniowych ( a0 ,..., an ) w najlepszym dopasowaniu do konkret-
x(k − 1) - oszacowane wartości wyznaczanych parametrów a priori z kroku poprzedniego,
P(k − 1) - oszacowane wartości macierzy kowariancji
nej reprezentacji zbioru liczbowego dla parametru Y .
Należy dodać, że liczba danych parametru X musi być
taka sama jak zbioru wejściowego Y . Stopień rozwinięcia wielomianu zależy w głównej mierze od liczby danych wejściowych zbioru liczbowego Y oraz trendu
zmian parametru Y . Wysoki stopień wielomianu określa
lepsze dopasowanie do danych źródłowych ze zbioru Y
oraz umożliwia wygładzenie pomiarów odstających ze
zbioru Y . W analizowanym przypadku zaproponowano
zastosowanie wielomianu 9-ego stopnia, jak poniżej:
a priori z kroku poprzedniego,
x (k − 1) - prognoza wartości stanu,
P(k − 1) - prognozowane wartości macierzy kowariancji,
Q(k − 1) - macierz kowariancji procesu szumu,
 qψ
Q (k − 1) = 
0
(q
ψ
0 
,
qdψ 
, qdψ ) - wariancje szumu procesu pomiarowego dla
Y = a0 ⋅ X + a1 ⋅ X + ... + a8 ⋅ X + a9 ⋅ X
pojedynczego kąta i jego dryftu (przykład podany
dla kąta Heading).
gdzie:
( a0 , a1..., a8 , a9 ) - wyznaczane współczynniki wielomianu 9-
2) II etap- proces “korekcji”:
(
K (k ) = P(k − 1) ⋅ H T ⋅ H ⋅ P(k − 1) ⋅ H T + R
(
x( k ) = x( k − 1) + K ( k ) ⋅ z − H ⋅ x( k − 1)
)
−1
)
P ( k ) = ( I − K ( k ) ⋅ H ) ⋅ P ( k − 1)
ego stopnia (w sumie 10 współczynników liniowych).
Równanie (10), przy założeniu iż liczba zbioru wejściowego Y jest znacznie większa od liczby wyznaczanych
współczynników, jest rozwiązywane metodą najmniejszych kwadratów, jak poniżej [10]:
(6)
(7)
(8)


-1
 Qx = N ⋅ L

v = A ⋅ Qx - dl

 m = [ vv ]
 0
r−s
gdzie:
R - macierz kowariancji pomiarów,
H - macierz pochodnych cząstkowych,
H = [1 0] ,
(11)
gdzie:
K (k ) - macierz wzmocnienia Kalmana,
Qx - wektor z wyznaczanymi współczynnikami liniowymi
z - wektor wielkości pomierzonych,
wielomianu,
I - macierz jednostkowa,
N = AT ⋅ A - układ równań normalnych,
x(k ) - wyznaczane parametry a posteriori,
A - macierz współczynników,
P(k ) - macierz kowariancji wyznaczanych parame-
L = AT ⋅ dl ,
trów a posteriori.
dl - wektor wyrazów wolnych,
2.2 METODA WIELOMIANOWA
m0 - odchylenie standardowe poprawek,
Podstawowe równanie modelu matematycznego dla
metody wielomianowej przyjmuje postać [5]:
Y = a0 ⋅ X + ... + an ⋅ X
r - liczba obserwacji zbioru wejściowego, r > 10 ,
s - liczba wyznaczanych współczynników, s = 10 ,
(9)
v - wektor poprawek.
gdzie:
2.3 METODA TRYGONOMETRYCZNA
Y - źródłowe dane parametru, pozyskane z określonego
sensora,
( a0 ,..., an ) -
(10)
Metoda trygonometryczna umożliwia dopasowanie
źródłowych danych wejściowych Y z wykorzystaniem
funkcji parzystej cosinus lub funkcji nieparzystej sinus.
W pracy zaproponowano zastosowanie funkcji parzystej
cosinus do korekcji kątów HPR, jak poniżej [9]:
wyznaczane współczynniki liniowe wielomia-
nu,
n - stopień wielomianu,
Y = c0 ⋅ cos ( 0 ⋅ X ⋅ π ) + ... + c3 ⋅ cos ( 6 ⋅ X ⋅ π ) + ...
X - argumenty funkcji wielomianowej (np. numery
kolejnych epok lub interwał czasu).
+c5 ⋅ cos (10 ⋅ X ⋅ π )
134
(12)
DAMIAN WIERZBICKI, KAMIL KRASUSKI
gdzie:
- błąd pomiaru kąta Heading: mψ = ±1.50 ,
( c0 , c1 , c2 , c3 , c4 , c5 ) -
wyznaczane współczynniki funkcji
- błąd pomiaru kąta Pitch: mθ = ±1.50 ,
trygonometrycznej (w sumie 6 współczynników).
- błąd pomiaru kąta Roll: mφ = ±20 ,
Funkcja trygonometryczna z równania (12) została
określona poprzez użycie krotności funkcji bazowej
cos( j ⋅ X ⋅ π ) , w której parametr „j” oznacza liczby
- wartość początkowa macierzy kowariancji dla kąta i
jego dryftu (macierz zastosowana dla wszystkich kątów
105 0 
P( k = 1) = 
2
 0 10  ,
HPR):
- wartości wariancji procesu pomiarowego (macierz
zastosowana
dla
wszystkich
kątów
HPR):
 0.1 0 
Q ( k = 1) = 
,
 0 0.01
całkowite z przedziału j = [0;2;4;6;8;10] . Równanie (12)
jest rozwiązywane z zastosowaniem metody najmniejszych kwadratów (patrz równanie 11 w rozdziale 2.2), z
tymże liczba wyznaczanych parametrów wynosi 6 ( s = 6
) oraz minimalna liczba obserwacji źródłowych dla
zbioru wejściowego Y wynosi powyżej 6 ( r > 6 ).
- liczba epok pomiarowych: nk = 85 ;
II metoda wielomianowa:
- liczba wyznaczanych współczynników wynosi 10,
- metoda obliczeń: metoda najmniejszych kwadratów,
- liczba epok pomiarowych: nk = 85 ,
- rząd macierzy współczynników A wynosi 10,
- obliczenia numeryczne realizowane niezależnie dla
każdego kąta obrotu HPR;
III metoda trygonometryczna:
- wyrażenie funkcji trygonometrycznej: krotność funkcji
cosinus,
- liczba wyznaczanych współczynników wynosi 6,
- metoda obliczeń: metoda najmniejszych kwadratów,
- liczba epok pomiarowych: nk = 85 ,
- rząd macierzy współczynników A wynosi 6,
- obliczenia numeryczne realizowane niezależnie dla
każdego kąta obrotu HPR.
3. EKSPERYMENT BADAWCZY
W części praktycznej eksperymentu badawczego dokonano korekcji wartości kątów HPR, które zostały
zarejestrowane w trakcie przelotu testowego przez
urządzenia Trimble UX-5 (jeden z rodzajów BSP).
Urządzenie Trimble UX-5 rejestruje automatycznie kąty
rotacji HPR i zapisuje je w pliku tekstowym (tzw.
„log”). Wysokość elipsoidalna lotu BSP wynosiła od
222.1 m do 235.9, przy średniej wartości około 230 m.
4. WYNIKI EKSPERYMENTU
BADAWCZEGO
DYSKUSJA
W rozdziale czwartym prezentowanego artykułu
przedstawiono i opisano rezultaty przeprowadzonych
badań. W postaci graficznej zaprezentowano parametry
finalne filtracji Kalmana (odchylenia standardowe kątów
HPR oraz wartości dryftu kątów HPR), a ponadto
porównano wartości kątów obrotu HPR po korekcji dla
trzech metod badawczych.
Rys. 1. Trajektoria pozioma i pionowa BSP
Modele matematyczne dla filtracji Kalmana, metody
wielomianowej i metody trygonometrycznej zostały
zaimplementowane do programu Scilab 5.4.1, w którym
wykonano obliczenia korekcji kątów HPR. W trakcie
przeprowadzania obliczeń numerycznych przyjęto następujące parametry konfiguracji dla parametrów wejściowych modelu:
I
Na rys. 2 zaprezentowano wartości błędów średnich
(odchylenia standardowe) dla kątów HPR po filtracji
Kalmana. Wartość przeciętna dokładności kątów Heading i Pitch wynosi 0.920, przy rozrzucie wyników od
0.880 do 1.500. Wartość przeciętna dokładności kąta Roll
wynosi 1.150, przy rozrzucie wyników od 1.090 do 2.000.
Porównując błędy średnie pomiędzy poszczególnymi
kątami HPR warto zauważyć, iż dokładność wyznaczenia kątów Heading i Pitch jest wyższa o około 20%
względem dokładności kąta Roll.
metoda filtracji Kalmana:
- okres próbkowania obserwacji: ∆t = 1 sekunda,
- wartość początkowa parametrów wektora
x ( k = 1) = [0;0]T ,
stanu
135
METODY KOREKCJI KĄTÓW OBROTU HEADING, PITCH, ROLL (...)
filtracją Kalmana a danymi źródłowymi z sensora Trimble UX-5 wynoszą od -13.490 do +9.020. Różnica wyników kąta Heading pomiędzy metodą wielomianową a
danymi źródłowymi z sensora Trimble UX-5 wynoszą od
-17.520 do +12.100. Różnica wyników kąta Heading
pomiędzy metodą trygonometryczną a danymi źródłowymi z sensora Trimble UX-5 wynoszą od -16.930 do
+12.590. Należy zauważyć, że dyspersja rezultatów
porównania wartości kąta Heading z surowych odczytów
i poszczególnej metody korekcji jest najmniejsza dla
metody filtracji Kalmana Dla każdej metody badawczej
określono również odchylenie standardowe dla różnicy
kąta Heading z danych źródłowych i metody korekcji.
Wartość odchylenia standardowego dla różnicy wartości
kąta Heading przed i po filtracji Kalmana wynosi 3.340.
Z kolei dla metody wielomianowej i trygonometrycznej,
wartość odchylenia standardowego dla różnicy wartości
kąta Heading przed i po korekcji wynosi odpowiednio
4.210 oraz 4.310.
Rys. 2. Dokładność kątów HPR po filtracji Kalmana
Na rys. 3 zaprezentowano wartości dryftu dla
wszystkich kątów HPR w funkcji epoki pomiarowej.
Przeciętna wartość dryftu dla kąta Heading wynosi -0,04
[0/s2], dla przedziału wyników od -6,60 [0/s2] do 1,09
[0/s2]. Przeciętna wartość dryftu dla kąta Pitch wynosi
0,14 [0/s2], dla przedziału wyników od -0,67 [0/s2] do 1,99
[0/s2]. Przeciętna wartość dryftu dla kąta Roll wynosi 0,07 [0/s2], dla przedziału wyników od -1,93 [0/s2] do 0,32
[0/s2].
Na rys. 5 zaprezentowano wartości kąta Pitch na
podstawie danych źródłowych oraz metod korekcji w
funkcji epoki pomiarowej. Średnia wartość kąta Pitch na
podstawie surowych odczytów wynosi 4.740 z odchyleniem standardowym 3.380. W przypadku filtracji Kalmana, średnia wartość kąta Pitch wynosi 4.750 z odchyleniem standardowym 2.510. W metodzie wielomianowej
i trygonometrycznej, średnia wartość kąta Pitch wynosi
4.740, jednakże odchylenia standardowe wynoszą odpowiednio dla danej metody 1.300 oraz 1.060. Trzeba
nadmienić, iż w każdej metodzie badawczej odchylenie
standardowe dla wartości średniej kąta Pitch jest znacznie mniejsze od 3.380. Dla filtracji Kalmana, metody
wielomianowej, metody trygonometrycznej odchylenia
standardowe wartości średniej kąta Pitch zostały zredukowane odpowiednio o 26%, 62% oraz 67%. Różnica
wyników kąta Pitch pomiędzy filtracją Kalmana a
danymi źródłowymi z sensora Trimble UX-5 wynoszą od
-6.480 do +4.780.
Rys. 3. Wartości dryftu dla kątów HPR
Na rys. 4 zaprezentowano wartości kąta Heading na
podstawie danych źródłowych oraz metod korekcji w
funkcji epoki pomiarowej. Średnia wartość kąta Heading
na podstawie surowych odczytów wynosi 175.770 z
odchyleniem standardowym 4.520. W przypadku filtracji
Kalmana, średnia wartość kąta Heading wynosi 175.740 z
odchyleniem standardowym 2.340. W metodzie wielomianowej i trygonometrycznej, średnia wartość kąta
Heading wynosi 175.770, jednakże odchylenia standardowe wynoszą odpowiednio dla danej metody 1.630 oraz
1.360. Należy nadmienić, iż w każdej metodzie badawczej
odchylenie standardowe dla wartości średniej kąta
Heading jest znacznie mniejsze od 4.520. Dla filtracji
Kalmana, metody wielomianowej, metody trygonometrycznej odchylenia standardowe wartości średniej kąta
Heading zostały zredukowane odpowiednio o 47%, 64%
oraz 70%. Różnica wyników kąta Heading pomiędzy
Rys. 4. Wartości kąta Heading na podstawie danych
źródłowych i metod korekcji
136
DAMIAN WIERZBICKI, KAMIL KRASUSKI
trygonometrycznej, średnia wartość kąta Roll wynosi
0.160, jednakże odchylenia standardowe wynoszą odpowiednio dla danej metody 0.590 oraz 0.620. Zaobserwowano, iż w każdej metodzie badawczej odchylenie standardowe dla wartości średniej kąta Roll jest znacznie
mniejsze od 2.700. Dla filtracji Kalmana, metody wielomianowej, metody trygonometrycznej odchylenia standardowe wartości średniej kąta Roll zostały zredukowane odpowiednio o 55%, 78% oraz 77%. Różnica wyników
kąta Roll pomiędzy filtracją Kalmana a danymi źródłowymi z sensora Trimble UX-5 wynoszą od -4.990 do
+5.090. Różnica wyników kąta Roll pomiędzy metodą
wielomianową a danymi źródłowymi z sensora Trimble
UX-5 wynoszą od -6.020 do +7.260. Różnica wyników
kąta Roll pomiędzy metodą trygonometryczną a danymi
źródłowymi z sensora Trimble UX-5 wynoszą od -6.960
do +6.650. Należy zauważyć, iż dyspersja rezultatów
porównania wartości kąta Roll z surowych odczytów i
poszczególnej metody korekcji jest najmniejsza dla
metody filtracji Kalmana. Dla każdej metody badawczej
określono również odchylenie standardowe dla różnicy
kąta Roll z danych źródłowych i metody korekcji. Wartość odchylenia standardowego dla różnicy wartości kąta
Roll przed i po filtracji Kalmana wynosi 2.140. Z kolei
dla metody wielomianowej i trygonometrycznej, wartość
odchylenia standardowego dla różnicy wartości kąta Roll
przed i po korekcji wynosi odpowiednio 2.640 oraz 2.630.
Rys. 5. Wartości kąta Pitch na podstawie danych źródłowych i metod korekcji
Różnica wyników kąta Pitch pomiędzy metodą wielomianową a danymi źródłowymi z sensora Trimble UX-5
wynoszą od -7.080 do +6.230. Różnica wyników kąta
Pitch pomiędzy metodą trygonometryczną a danymi
źródłowymi z sensora Trimble UX-5 wynoszą od -7.380
do +6.090. Należy zauważyć, że dyspersja rezultatów
porównania wartości kąta Pitch z surowych odczytów i
poszczególnej metody korekcji jest najmniejsza dla
metody filtracji Kalmana. Dla każdej metody badawczej
określono również odchylenie standardowe dla różnicy
kąta Pitch z danych źródłowych i metody korekcji.
Wartość odchylenia standardowego dla różnicy wartości
kąta Pitch przed i po filtracji Kalmana wynosi 2.400. Z
kolei dla metody wielomianowej i trygonometrycznej,
wartość odchylenia standardowego dla różnicy wartości
kąta Pitch przed i po korekcji wynosi odpowiednio 3.120
oraz 3.210.
5. WNIOSKI
W artykule opisano i zaprezentowano rezultaty korekcji kątów HPR dla BSP z użyciem algorytmu filtracji
Kalmana, metody wielomianowej i trygonometrycznej.
Obliczenia numeryczne wykonano na danych źródłowych
HPR, zarejestrowanych przez urządzenie Trimble UX-5.
Kod źródłowy programu obliczeniowego został napisany
w programie Scilab 5.4.1. Na podstawie przeprowadzonych obliczeń i eksperymentów badawczych wyciągnięto
następujące wnioski:
- zastosowanie filtru Kalmana pozwala zmniejszyć błędy
średnie (odchylenia standardowe) wyznaczonych kątów
rotacji HPR odpowiednio: z poziomu 20 na 1.150 dla kąta
Roll oraz z poziomu 1.50 do 0.920 dla kątów Heading i
Pitch;
- zastosowanie dwuwymiarowego modelu systemu pomiarowego dla algorytmu filtru Kalmana pozwala na
wyznaczenie dryftu kątów obrotu HPR;
Rys. 6. Wartości kąta Roll na podstawie danych źródłowych i metod korekcji
- wartość odchylenia standardowego dla różnicy wyników pomiędzy surowymi odczytami kąta Heading oraz
rezultatami korekcji filtracji Kalmana, metody wielomianowej, metody trygonometrycznej wynosi odpowiednio 3.340, 4.210 i 4.310;
Na rys. 6 zaprezentowano wartości kąta Roll na podstawie danych źródłowych oraz metod korekcji w funkcji
epoki pomiarowej. Średnia wartość kąta Roll na podstawie surowych odczytów wynosi 0.160 z odchyleniem
standardowym 2.700. W przypadku filtracji Kalmana,
średnia wartość kąta Roll wynosi 0.220 z odchyleniem
standardowym 1.220. W metodzie wielomianowej i
- wartość odchylenia standardowego dla różnicy wyników pomiędzy surowymi odczytami kąta Pitch oraz
rezultatami korekcji filtracji Kalmana, metody wielo-
137
METODY KOREKCJI KĄTÓW OBROTU HEADING, PITCH,
PITCH, ROLL (...)
mianowej, metody trygonometrycznej wynosi odpowiedodpowie
nio 2.400, 3.120 i 3.210;
wyn
- wartość odchylenia standardowego dla różnicy wyników pomiędzy surowymi odczytami kąta Roll oraz
rezultatami korekcji filtracji Kalmana, metody wielowiel
mianowej, metody
dy trygonometrycznej wynosi odpowiedodpowie
nio 2.140, 2.640 i 2.630.
Literatura
1. Bieda R., Grygiel R.: Wyznaczanie orientacji obiektu w przestrzeni z wykorzystaniem naiwnego filtru Kalmana.
Kalmana
„Przegląd Elektrotechniczny”” 2014, R. 90, nr 1, s. 34-41.
2. Kędzierski J.: Filtr Kalmana - zastosowania w prostych układach sensorycznych.
sensorycznych W: Koło Naukowe Robotyków
„Konar” 2007, s. 24-30.
3. Kędzierski M., Wierzbicki D., Wilińska M., Fryśkowska A.:
A. Analiza możliwości wykonania aerotriangulacji zdjęć
cyfrowych pozyskanych kamerą niemetryczną zamontowaną na pokładzie bezzałogowego statku latającego bez
systemu GPS/INS. Biuletyn WAT 2013, vol.
v LXII, nr 4, s. 241-252.
4. Kędzierski M., Fryśkowska A., Wierzbicki D.:
D. Opracowania fotogrametryczne
ogrametryczne z niskiego pułapu.
pułapu Warszawa: WAT,
2014., s. 34-36. ISBN 978-83-7938-047
047-3.
5. Kiusalaas J.: Numerical methods in engineering with MATLAB.
MATLAB 2th ed. Published in the United States of America
by Cambridge University Press, New York, 2009,
2009 p. 128-130. ISBN-13 978-0-511-64033-9..
6. Kolecki J., Prochaska M., Piątek P., Baranowski J., Kurczyński Z.:
Z. Stabilizacja systemu pomiarowego dla wiatrawiatr
kowca w aspekcie jakości LIDAR.. „Archiwum
„
Fotogrametrii, Kartografii i Teledetekcji”” 2015, vol. 27, s. 71-82,
2015. DOI: 10.14681/afkit.2015.005.
7. Krasuski K., Wierzbicki D.: Wyznaczenie kursu bezzałogowego statku powietrznego na podstawie danych GPS i
INS. „Pomiary
Pomiary Automatyka Robotyka”
Robotyka 2015, R. 19, nr 4/2015, s. 63–68. DOI: 10.14313/PAR_218/63.
8. Nowak A., Naus K.: Badanie możliwości określania parametrów ruchu statku za pomocą systemu EGNOS.
EGNOS „Logistyka” 2014, nr 6, s. 7923-7932.
9. Ratajczak T.: Metody numeryczne: przykłady i zadania.
zadania Gdańsk: Wyd. Pol. Gd., 2006, s. 120 -121.
10. Subirana J.S., Zornoza J. M.J., Hernández-Pajares
Hernández
M.: GNSS data processing. Vol. I: Fundamentals and algorithms. ESA Communications, ESTEC, Noordwijk, Netherlands, 2013,
2013 p. 141-141. ISBN: 978-92-9221-886-7.
978
11. Yi Y.: On improving the accuracy and reliability of GPS/INS-based
based direct sensor georeferencing.
georeferencing Ph. D. Thesis,
2007. Ohio State University, p. 37-40.
40.
Ten artykuł dostępny jest na licencji Creative Commons Uznanie autorstwa 3.0 Polska.
Pewne prawa zastrzeżone na rzecz autorów.
Treść licencji jest dostępna na stronie http://creativecommons.org/licenses/by/3.0/pl/
138