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: mmaka@am.szczecin.pl
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 ekspe-rymentu polegającego na integracji równoległych pomiarów pozycji, pochodzących z dwóch różnych odbior-nikó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
Podstawą dobrej praktyki morskiej jest bez-pieczne prowadzenie nawigacji, czyli bezbez-pieczne 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 ste-rowanego 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 ryzy-ka jest zainstalowanie systemu określania pozycji
wykorzystującego kilka niezależnych źródeł infor-macji 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 ogra-niczenia błędów pomiarowych.
Główne zalety systemu określania pozycji wy-korzystującego filtrację oraz integrację danych nawigacyjnych z wielu źródeł to:
− 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 zastosowa-nie systemu działającego w oparciu o algorytm wielosensorowej fuzji danych z wykorzystaniem filtru Kalmana. 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.
Opis algorytmu
Rozważaniu poddano dyskretny system stocha-styczny z kilkoma sensorami (czujnikami):
( )
( ) ( )
( )
t( )
t( )
t i l t t t i i i 1,2,... 1 = + ⋅ = + ⋅ = + v x H y w x Φ x (1) gdzie: x(t)∈Rn – wektor stanu,( )
mi i t ∈Ry – wektor pomiarów i-tego sensora
(1 ≤ mi≤ n),
Φ, Hi – stałe macierze o odpowiednich wymiarach,
w(t), vi(t) – wektory zakłóceń o charaktery-styce białych szumów gaussow-skich z zerowymi wartościami oczekiwanymi oraz macierzami kowariancji odpowiednio Q i Ri. Fuzję zbioru danych l sensorów wyraża się średnią ważoną [1]:
( )
t A( ) ( )
t x t A( ) ( )
t x t Al( ) ( )
t xl t x ˆ ˆ ... ˆ ~ 2 2 1 1 ⋅ + ⋅ + + ⋅ = (2) gdzie:Ai(t) – macierze wag,
( )
tx~ – wektor fuzji estymat stanu,
( )
ti
xˆ – estymaty wektora stanu.
Estymaty wektora stanu xˆi
( )
t dla i-tego podsys-temu (określonego danym sensorem) uzyskuje się przy zastosowaniu filtru Kalmana [2]:(
)
(
)
( )
( )
(
)
(
)
(
)
( )
(
)
[
(
)
]
( )
(
)
( )
(
)
( )
t t(
t t)
( ) ( )
t t t t t t t t t t t t t t t t t t t t t t t t t t i i i i i i i i i i T i i i T i i i T i i i i i i i i E K x x P H K P P R H P H H P K Q Φ P Φ P x H y E x Φ x ⋅ + − = − ⋅ ⋅ − − = + ⋅ − ⋅ ⋅ ⋅ − = + ⋅ − − ⋅ = − − ⋅ − = − − ⋅ = − − 1 \ ˆ \ ˆ 1 \ 1 \ \ 1 \ 1 \ 1 \ 1 1 \ 1 \ ˆ 1 \ 1 ˆ 1 \ ˆ 1 (3) gdzie:(
\ 1)
ˆi t t−x – estymata wektora stanu
wyzna-czona bez znajomości pomiarów w chwili t,
Ei(t) – wektor innowacji w chwili t, Pi(t \ t – 1) – macierz kowariancji błędów
fil-tracji wyznaczona bez znajomości wzmocnienia filtru w chwili t,
Ki(t) – macierz wzmocnienia filtru
w chwili t,
Pi(t \ t) – uaktualniona macierz kowariancji
błędów filtracji,
( )
t ti \
ˆx – uaktualniona estymata wektora stanu.
Macierze wag wyznacza się ze wzoru [3, 4, 5]:
( )
t( )
t( )
t A l ii j jj i 1 1 1 1 − − = − ⋅ =∑
P P (4) 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).
Macierze krzyżowej kowariancji błędów filtracji wyznacza się ze wzoru [4, 5, 6]:
( )
[
( )
]
[
( )
]
( )
[
]
T j j n T ij i i n ij H t K I Q t P H t K I t P ⋅ − ⋅ ⋅ + Φ ⋅ − ⋅ Φ ⋅ ⋅ − = 1 (5) gdzie:In – macierz jednostkowa o wymiarach n×n.
Zdefiniowany za pomocą równań (2)–(5) algo-rytm jest optymalny, gdyż minimalizuje ślad macie-rzy wariancji błędu estymatora fuzji [4, 5].
Badania
Algorytm fuzji danych nawigacyjnych został zweryfikowany poprzez zaimplementowanie go w postaci aplikacji działającej w czasie rzeczywi-stym (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 jed-nego 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 esty-macji:
( ) ( ) ( )
( ) ( ) ( )
t y t y t e t x t x t e y x ~ ~ − = − = (6) oraz błędy pomiarowe:( ) ( )
( )
( ) ( )
t yt y( )
t e t x t x t e i yi i xi ( ( − = − = 1 ≤ i ≤ lGPS (7) gdzie: GPSl
– liczba odbiorników GPS.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)
x [m] y [m] ex , ex1 , ex2 [m] ey , ey1 , ey2 [m]
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 qualityand of track measurements (s = 300)
Jx + Jy , Jx1 + Jy1 [m 2 ] Jy , Jy1 , Jy2 [m 2 ] Jx , Jx1 , Jx2 [m 2 ] x [m]
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)
Za wskaźniki jakości estymacji współrzędnych kartezjańskich wybrano kryteria kwadratowe (czym mniejsza wartość wskaźnika, tym lepsza jakość estymacji):
( )
( )
∫
∫
= = s y y s x x t t e J t t e J 0 2 0 2 d d (8) gdzie:s– ilość cykli estymacji.
Podobnie zdefiniowano wskaźniki jakości po-miarów współrzędnych kartezjańskich:
( )
( )
∫
∫
= = s yi yi s xi xi t t e J t t e J 0 2 0 2 d d 1 ≤ i ≤ lGPS (9)Ocenę jakości estymacji trajektorii ruchu można dokonać, porównując wskaźniki:
1 1 1 x y y x J J J J J J + = + = (10) Na rysunkach 1–5 przedstawiono przykładowe
wyniki przeprowadzonych badań.
Uzyskane wyniki badań jednoznacznie potwier-dzają 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ładniej-szą estymację oraz szybdokładniej-szą stabilizację wartości wskaźnika jakości estymacji trajektorii.
Podsumowanie
Otrzymane wyniki badań potwierdzają skutecz-ność przedstawionego w artykule algorytmu inte-gracji danych nawigacyjnych.
Zaproponowana metoda integracji i filtracji da-nych 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ścio-wych 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 przydat-ności do wyznaczania pozycji obiektów poruszają-cych się.
Bibliografia
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,
War-szawa 1996.
3. STATECZNY A., LISAJ A., MOHAMMAD C.: Fuzja danych
nawigacyjnych w przestrzeni filtru Kalmana. Materiały Międzynarodowej Konferencji Explo-Ship, Zeszyty Na-ukowe AM, 2006, 11(83), 279–286.
4. SUN S.: Multi-sensor Optimal Information Fusion Kalman
Filters with Application. Aerospace Science and Technol-ogy, 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.
Recenzent: prof. dr hab. inż. Andrzej Stateczny Akademia Morska w Szczecinie
Jx + Jy , Jx1 + Jy1 [m 2 ]
