Pobierz

Scientific Journals
Zeszyty Naukowe
Maritime University of Szczecin
Akademia Morska w Szczecinie
2008, 13(85) pp. 5‐9
2008, 13(85) s. 5‐9
Pozycjonowanie bazujące na wielosensorowym filtrze Kalmana
Positioning based on the multi-sensor Kalman filter
Piotr Borkowski, Janusz Magaj, Marcin Mąka*
Akademia Morska w Szczecinie, Instytut Nawigacji Morskiej
70-500 Szczecin, ul. Wały Chrobrego 1–2, *tel. 091 48 09 566, *e-mail: [email protected]
Słowa kluczowe: wielosensorowy filtr Kalmana, fuzja danych nawigacyjnych
Abstrakt
W artykule przedstawiono algorytm fuzji danych nawigacyjnych. Algorytm został zweryfikowany poprzez
zaimplementowanie go w postaci aplikacji działającej w czasie rzeczywistym. Przedstawiono wyniki eksperymentu polegającego na integracji równoległych pomiarów pozycji, pochodzących z dwóch różnych odbiorników GPS.
Key words: multi-sensor Kalman filter, navigational data fusion
Abstract
This article discusses an algorithm of navigational data fusion. The algorithm has been verified through its
implementation in a real time application. Parallel measurements of position obtained from two different GPS
receivers have been integrated in this experiment. The experiment results are presented.
Wstęp
wykorzystującego kilka niezależnych źródeł informacji o położeniu sterowanego obiektu. Systemy
takie, chociaż ciągle udoskonalane, także wskazują
pozycję z określoną dokładnością, ale obarczoną
błędem. Jedną z metod redukcji tego błędu jest
zastosowanie systemu nawigacyjnego, w którym do
wyznaczenia dokładnej pozycji wykorzystuje się
dane uzyskiwane z wielu źródeł oraz poddaje je
obróbce: integracji i filtracji w celu dalszego ograniczenia błędów pomiarowych.
Główne zalety systemu określania pozycji wykorzystującego filtrację oraz integrację danych
nawigacyjnych z wielu źródeł to:
Podstawą dobrej praktyki morskiej jest bezpieczne prowadzenie nawigacji, czyli bezpieczne
i bezkolizyjne przeprowadzenie jednostki pływającej z jednego miejsca do drugiego. Zagadnienie to
sprowadza się do rozwiązania dwóch zadań: określenia pozycji i wytyczenia właściwego kursu sterowanego obiektu.
Koniec dwudziestego wieku to okres szybkiego
rozwoju satelitarnych systemów pozycjonowania.
W dobie nawigacji satelitarnej praktycznie każdy
statek jest wyposażony w odbiornik systemu GPS,
wskazujący współrzędne pozycji oraz zliczający
przebytą drogę. Jednakże opierając się tylko i wyłącznie na informacji nawigacyjnej otrzymywanej
z pojedynczych, autonomicznych odbiorników,
narażamy się na ryzyko wystąpienia znacznych
błędów lub straty informacji. Wynika to z wady
tych systemów, jaką jest możliwość stosunkowo
łatwego zakłócania ich pracy.
Sposobem na znaczne zredukowanie tego ryzyka jest zainstalowanie systemu określania pozycji
Zeszyty Naukowe 13(85)
−
−
−
−
zwiększenie niezawodności działania systemu,
zmniejszenie błędów pomiarów,
zapewnienie ciągłości pracy,
zwiększenie częstotliwości uzyskiwania danych.
Jednym z możliwych rozwiązań jest zastosowanie systemu działającego w oparciu o algorytm
wielosensorowej fuzji danych z wykorzystaniem
filtru Kalmana. Algorytm został zweryfikowany
poprzez zaimplementowanie go w postaci aplikacji
5
Piotr Borkowski, Janusz Magaj, Marcin Mąka
Ki(t) – macierz wzmocnienia filtru
w chwili t,
Pi(t \ t) – uaktualniona macierz kowariancji
błędów filtracji,
x̂ i (t \ t ) – uaktualniona estymata wektora
stanu.
działającej w czasie rzeczywistym. Przedstawiono
wyniki eksperymentu polegającego na integracji
równoległych pomiarów pozycji, pochodzących
z dwóch różnych odbiorników GPS.
Opis algorytmu
Rozważaniu poddano dyskretny system stochastyczny z kilkoma sensorami (czujnikami):
x(t + 1) = Φ ⋅ x(t ) + w (t )
y i (t ) = H i ⋅ x(t ) + v i (t ) i = 1,2,...l
Macierze wag wyznacza się ze wzoru [3, 4, 5]:
−1
 l

Ai (t ) = ∑ P jj−1 (t ) ⋅ Pii−1 (t )
 j =1

(1)
gdzie:
Pij(t) – macierz krzyżowej kowariancji błędów filtracji pomiędzy i-tym, a j-tym
podsystemem systemu określonego
równaniem (1).
gdzie:
x(t)∈Rn – wektor stanu,
y i (t ) ∈ R mi – wektor pomiarów i-tego sensora
(1 ≤ mi ≤ n),
Φ, Hi – stałe macierze o odpowiednich
wymiarach,
w(t), vi(t) – wektory zakłóceń o charakterystyce białych szumów gaussowskich z zerowymi wartościami
oczekiwanymi oraz macierzami
kowariancji odpowiednio Q i Ri.
Macierze krzyżowej kowariancji błędów filtracji
wyznacza się ze wzoru [4, 5, 6]:
[
⋅ I n − K j (t ) ⋅ H j
Badania
Algorytm fuzji danych nawigacyjnych został
zweryfikowany poprzez zaimplementowanie go
w postaci aplikacji działającej w czasie rzeczywistym (online).
Eksperyment badawczy polegał na integracji
(fuzji) równoległych pomiarów pozycji, pochodzących z dwóch różnych nieruchomych odbiorników
GPS (przy czym do celów integracji pomiary jednego z odbiorników GPS były przesuwane o znany
wektor translacji). Współrzędne geograficzne były
przekształcane na współrzędne kartezjańskie [6].
W celu oszacowania (oceny) jakości estymacji
współrzędnych pozycji zdefiniowano błędy estymacji:
ex (t ) = x(t ) − ~
x (t )
(6)
e (t ) = y (t ) − ~
y (t )
xˆ i (t \ t − 1) = Φ ⋅ xˆ i (t − 1 \ t − 1)
E i (t ) = y i (t ) − H i ⋅ xˆ i (t \ t − 1)
Pi (t \ t ) = Pi (t \ t − 1) − K i (t ) ⋅ H i ⋅ Pi (t \ t − 1)
xˆ i (t \ t ) = xˆ i (t \ t − 1) + K i (t ) ⋅ E i (t )
]
Zdefiniowany za pomocą równań (2)–(5) algorytm jest optymalny, gdyż minimalizuje ślad macierzy wariancji błędu estymatora fuzji [4, 5].
Ai(t) – macierze wag,
~
x (t ) – wektor fuzji estymat stanu,
x̂ i (t ) – estymaty wektora stanu.
Estymaty wektora stanu x̂ i (t ) dla i-tego podsystemu (określonego danym sensorem) uzyskuje się
przy zastosowaniu filtru Kalmana [2]:
[
]
T
(5)
gdzie:
In – macierz jednostkowa o wymiarach n×n.
gdzie:
K i (t ) = Pi (t \ t − 1) ⋅ H Ti ⋅ H i ⋅ Pi (t \ t − 1) ⋅ H Ti + R i
[
Pij (t ) = [I n − K i (t ) ⋅ H i ]⋅ Φ ⋅ Pij (t − 1) ⋅ Φ T + Q ⋅
Fuzję zbioru danych l sensorów wyraża się
średnią ważoną [1]:
~
x (t ) = A1 (t ) ⋅ xˆ 1 (t ) + A 2 (t ) ⋅ xˆ 2 (t ) + ... + A l (t ) ⋅ xˆ l (t )
(2)
Pi (t \ t − 1) = Φ ⋅ Pi (t − 1 \ t − 1) ⋅ ΦT + Q
(4)
]
−1
(3)
gdzie:
xˆ i (t \ t − 1) – estymata wektora stanu wyznaczona bez znajomości pomiarów
w chwili t,
Ei(t) – wektor innowacji w chwili t,
Pi(t \ t – 1) – macierz kowariancji błędów filtracji wyznaczona bez znajomości
wzmocnienia filtru w chwili t,
y
oraz błędy pomiarowe:
(
exi (t ) = x(t ) − xi (t )
(
e yi (t ) = y (t ) − yi (t )
1 ≤ i ≤ lGPS
(7)
gdzie:
lGPS – liczba odbiorników GPS.
6
Scientific Journals 13(85)
y [m]
x [m]
Pozycjonowanie bazujące na wielosensorowym filtrze Kalmana
ey, ey1, ey2 [m]
ex, ex1, ex2 [m]
Rys. 1. Współrzędne kartezjańskie (s = 300)
Fig. 1. Cartesian coordinates (s = 300)
Rys. 2. Błędy estymacji i błędy pomiarowe współrzędnych kartezjańskich (s = 300)
Fig. 2. Estimation errors and measurement errors of Cartesian coordinates (s = 300)
Zeszyty Naukowe 13(85)
7
Jy, Jy1, Jy2 [m2]
Jx, Jx1, Jx2 [m2]
Piotr Borkowski, Janusz Magaj, Marcin Mąka
x [m]
Jx + Jy, Jx1 + Jy1 [m2]
Rys. 3. Wskaźniki jakości estymacji i wskaźniki jakości pomiarów współrzędnych kartezjańskich (s = 300)
Fig. 3. Estimation quality indicators and Cartesian coordinates measurement quality indicators (s = 300)
Rys. 4. Trajektoria ruchu oraz wskaźniki jakości estymacji i pomiarów trajektorii ruchu (s = 300)
Fig. 4. Movement trajectory and the indicators of estimation quality and of track measurements (s = 300)
8
Scientific Journals 13(85)
Jx + Jy, Jx1 + Jy1 [m2]
Pozycjonowanie bazujące na wielosensorowym filtrze Kalmana
Rys. 5. Wskaźniki jakości estymacji i pomiarów trajektorii ruchu (s = 7000)
Fig. 5. The indicators of estimation quality and of track measurements (s = 7000)
Podsumowanie
Za wskaźniki jakości estymacji współrzędnych
kartezjańskich wybrano kryteria kwadratowe (czym
mniejsza wartość wskaźnika, tym lepsza jakość
estymacji):
Otrzymane wyniki badań potwierdzają skuteczność przedstawionego w artykule algorytmu integracji danych nawigacyjnych.
Zaproponowana metoda integracji i filtracji danych umożliwia połączenie w pojedynczy sygnał
informacji pozyskanych z kilku autonomicznych
urządzeń pomiarowych oraz zmniejszenie błędu
pomiaru pozycji sterowanego obiektu. Zapewnia
także zwiększenie niezawodności działania systemu
poprzez uzyskiwanie danych z wielu sensorów, co
pozwala na eliminację błędnych danych wejściowych powstałych np. wskutek uszkodzenia układu
pomiarowego.
Przedstawione w niniejszym artykule analizy są
punktem wyjścia do dalszych badań mających na
celu m.in. określenie wpływu ilości sensorów na
dokładność określania pozycji z wykorzystaniem
zaproponowanego algorytmu oraz jego przydatności do wyznaczania pozycji obiektów poruszających się.
s
J x = ∫ e x2 (t )d t
0
s
Jy = ∫
(8)
e 2y
(t )d t
0
gdzie:
s– ilość cykli estymacji.
Podobnie zdefiniowano wskaźniki jakości pomiarów współrzędnych kartezjańskich:
s
J xi = ∫ exi2 (t )d t
0
s
J yi = ∫
e 2yi
(t )d t
1 ≤ i ≤ lGPS
(9)
0
Bibliografia
Ocenę jakości estymacji trajektorii ruchu można
dokonać, porównując wskaźniki:
J = Jx + Jy
J1 = J x1 + J y1
1. STATECZNY A.: AIS and Radar Data Fusion in Maritime
Navigation. Materiały Międzynarodowej Konferencji
Explo-Ship, Zeszyty Naukowe AM, 2004, 2(74), 329–336.
2. KACZOREK T.: Teoria sterowania i systemów. PWN, Warszawa 1996.
3. STATECZNY A., LISAJ A., MOHAMMAD C.: Fuzja danych
nawigacyjnych w przestrzeni filtru Kalmana. Materiały
Międzynarodowej Konferencji Explo-Ship, Zeszyty Naukowe AM, 2006, 11(83), 279–286.
4. SUN S.: Multi-sensor Optimal Information Fusion Kalman
Filters with Application. Aerospace Science and Technology, 2004, 8, 1, 2004, 57–62.
5. SUN S., DENG Z.: Multi-sensor Optimal Information Fusion
Kalman Filter. Automatica, 2004, 40, 1017–1023.
6. HLIBOWICKI R.: Geodezja wyższa i astronomia geodezyjna.
PWN, Warszawa 1981.
(10)
Na rysunkach 1–5 przedstawiono przykładowe
wyniki przeprowadzonych badań.
Uzyskane wyniki badań jednoznacznie potwierdzają wysoką jakość estymacji mierzonych sygnałów. Warto podkreślić, że od pewnego cyklu
(s = 3000) wartość wskaźnika jakości estymacji
trajektorii stabilizuje się, w przeciwieństwie do
wartości wskaźników jakości pomiarów (rys. 5).
Można również wnioskować, że większa liczba
odbiorników GPS będzie implikowała dokładniejszą estymację oraz szybszą stabilizację wartości
wskaźnika jakości estymacji trajektorii.
Zeszyty Naukowe 13(85)
Recenzent:
prof. dr hab. inż. Andrzej Stateczny
Akademia Morska w Szczecinie
9