Andrzej Banachowicz 1, Grzegorz Banachowicz 2 Fuzja pomiarów nawigacyjnych GPS/IS/DR Wstęp We współczesnym świecie pojęcie fuzji danych nabiera nowego znaczenia. Wieloźródłowość danych, bądź niewystarczająca liczba danych sprawiają, że opis interesującego nas wycinka rzeczywistości na satysfakcjonującym odbiorcę poziomie stał się o wiele trudniejszy. W odniesieniu do końca dwudziestego wieku i początku obecnego, mamy do czynienia z dynamicznym procesem integracji informacji nawigacyjnej oraz szeroko pojętej globalizacji danych i ich źródeł. Rozpowszechnienie globalnego systemu nawigacji, który znalazł zastosowanie w nawigacji morskiej, lotniczej (również kosmicznej) i lądowej, okazał się na tyle uniwersalny, że jego praktyczne wykorzystanie jest niemal nieograniczone. Wystarczy nadmienić, iż w geodezji, geologii czy leśnictwie i budownictwie określenie dokładnej pozycji jest pierwszorzędne. Wszystko to nie byłoby możliwe bez odpowiedniego poziomu technologicznego współczesnych systemów nawigacyjnych i informatycznych. Wysoka dokładność określania pozycji za pomocą systemów satelitarnych ora automatyzacja systemów nawigacji zliczeniowej (ang. DR dead reckoning) stawia duże wymagania w stosunku do przetwarzania danych nawigacyjnych. Często przyjmuje się, że obróbka danych pomiarowych ma na celu wyłącznie ich optymalne przetworzenie z punktu widzenia eliminacji zakłóceń (minimalizacji błędów określanych wielkości). Z tego względu najczęściej wykorzystywana jest estymacja danych nawigacyjnych, względnie parametrów rozkładów zakłóceń tych danych. Błędy nadmiarowe i systematyczne staramy się eliminować na etapie obróbki pierwotnej. Jednakże, rzeczywistość nie zawsze sprzyja takim założeniom, co dawniej uwidaczniane było podczas tzw. testów pomiarów, a obecnie jako sprawdzenie wiarygodności (integrity) urządzenia lub systemu nawigacyjnego. 1 dr hab. inż. Andrzej Banachowicz, Zachodniopomorski Uniwersytet Technologiczny w Szczecinie 2 mgr inż. Grzegorz Banachowicz, Policealna Szkoła Morska w Szczecinie, Towarzystwo Krzewienia Wiedzy o Morzu W artykule przedstawiono zagadnienia budowy różnych modeli nawigacji zintegrowanej drogą doboru odpowiedniej struktury filtru Kalmana modelu stanu i modelu pomiarów. Filtr Kalmana Metody filtracji Kalmanowskiej można stosować na różnych poziomach obróbki informacji nawigacyjnej. Poczynając od obróbki pierwotnej estymacji błędów pomiarów nawigacyjnych (na poziomie pomiaru wielkości fizycznych takich jak: faza, czas, amplituda itd.), a kończąc na estymacji współrzędnych pozycji oraz innych parametrów nawigacyjnych (wielkości geometrycznych). W każdym z tych przypadków posługujemy się takim samym algorytmem obliczeniowym. Ze względu na to, że współcześnie posługujemy się cyfrowymi układami pomiarowo-obliczeniowymi, to istotę tego algorytmu przedstawimy na przykładzie dyskretnego losowego układu dynamicznego. Opisują go dwa poniższe równania [Anderson, 1979], [Balakrishnan, 1984]: równanie stanu (model strukturalny) równanie pomiarów (model pomiarowy) gdzie: - n-wymiarowy wektor stanu,, (1), (2) - r-wymiarowy wektor zakłóceń stanu, - m-wymiarowy wektor pomiarów, - p-wymiarowy wektor zakłóceń pomiarów (szum pomiarowy), - n n-wymiarowa macierz przejścia, - m n-wymiarowa macierz pomiarów, r n, p m. 2
Ponadto dla wektorów zakłóceń w i v zakładamy, że są to szumy gaussowskie (o rozkładzie normalnym), o zerowym wektorze średnim i są wzajemnie nieskorelowane. Równanie stanu opisuje zmiany (trend) interesującego nas wektora, a model pomiarów podaje zależność funkcyjną pomiarów od tego wektora. Rozwiązaniem układu równań (1), (2), przy uwzględnieniu ograniczeń nałożonych na wektory zakłóceń, jest filtr Kalmana. Estymację wektora stanu w filtrze możemy przedstawić za pomocą poniższego schematu: prognoza wektora stanu, (3) gdzie wartość prognozowana wektora stanu, wartość estymowana wektora stanu, macierz kowariancji prognozowanego wektora stanu, (4) gdzie Q macierz kowariancji zakłóceń stanu (wektora w), proces innowacji macierz kowariancji procesu innowacji, (5), (6) gdzie R macierz kowariancji zakłóceń pomiarów (wektora v), macierz wzmocnienia filtru, (7) ocena (estymata) wektora stanu z filtracji po wykonaniu pomiaru, (8) macierz kowariancji estymowanego wektora stanu (9) Jak już wcześniej wspomniano algorytm obliczeniowy pozostaje ten sam, ale w konkretnych zastosowaniach będziemy mieli różne postacie i wymiary poszczególnych wektorów oraz macierzy. Poniżej przedstawiamy warianty rozwiązania nawigacji zintegrowanej oparte o różne modele strukturalne i pomiarowe. Przyjmując konkretny model nawigacji zintegrowanej musimy określić dwa równania: model strukturalny oraz model pomiarowy. Model strukturalny zdeterminowany jest przyjętym przez nas modelem procesu nawigacyjnego. Proces ten jest określony poprzez składowe wektora stanu oraz jego ewolucję (macierz A). Wektor stanu dobieramy w zależności od tego, jakie parametry chcemy estymować, tj. końcowe parametry nawigacyjne lub też ich błędy (składowe systematyczne w postaci poprawek). Ponadto musimy już z góry uwzględnić to, czy dysponujemy możliwością wykonywania pomiarów wielkości fizycznych pozostających w związku funkcyjnym z estymowanymi parametrami. Wynika z tego, że przy projektowaniu modelu strukturalnego musimy mieć co najmniej przybliżony obraz modelu pomiarowego. I w praktyce konstruowania zintegrowanych systemów nawigacyjnych tak postępujemy. Przyjmujemy wstępną koncepcję określającą jakie wielkości chcemy estymować i sprawdzamy, czy istnieją odpowiednie możliwości pomiarowe. Model pomiarowy (równanie 2) opisuje zależność pomiarów od wektora stanu. W przypadku deterministycznego obliczania współrzędnych pozycji (bez uwzględniania zakłóceń losowych stanu i pomiarów) lub estymacji metodą najmniejszych kwadratów zależność tą ujmujemy za pomocą macierzy Jacobiego (macierzy gradientów powierzchni pozycyjnych) [Banachowicz, 1991]. Powyższe rozważania zilustrujmy na przykładzie dwóch modeli nawigacji. W pierwszym wykorzystujemy pomiary nawigacji zliczeniowej (DR), satelitarnego systemu nawigacyjnego oraz naziemnego systemu radionawigacyjnego. W drugim modelu zastosowano tylko jeden system pozycyjny (GPS lub DGPS) oraz dwa układy nawigacji zliczeniowej log-żyrokompas i nawigację inercjalną (ang. INS inertial navigation system) [Banachowicz, 2001]. Fuzja pomiarów DR/GPS W tym przypadku dysponujemy pomiarami satelitarnego systemu nawigacyjnego (GPS, GLONASS, DGPS, DGLONASS), naziemnego systemu radionawigacyjnego (LORAN lub radionawigacyjny system bliskiego zasięgu) oraz pomiarami z logu i żyrokompasu. W nawigacji morskiej jako elementy wektora stanu przyjmujemy przede wszystkim współrzędne pozycji ( oraz ich pochodne, np. składowe wektora prędkości, wektora przyspieszeń itd. Załóżmy, że wielko- 3
ściami estymowanymi będą następujące parametry: współrzędne pozycji ( ), rzuty wektora prędkości względem dna na południk i równoleżnik ( ), błąd systematyczny kąta drogi względem dna ) (ang. COG Course Over Ground) oraz błąd systematyczny szybkości względem dna ( ) (ang. SOG Speed Over Ground). Przy tych założeniach wektor stanu będzie miał postać:. (10) Jak pamiętamy model strukturalny tworzy równanie stanu (wzór 1). Dlatego musimy określić także strukturę macierzy przejścia A. Przyjmijmy ją w następującej postaci: systemu radionawigacyjnego. Dla pomiarów synchronicznych zazwyczaj przyjmujemy w uproszczeniu, że t i = 1 sekunda jest to zazwyczaj stosowane w przypadku pomiarów synchronicznych, taktowanych z odbiornika GPS. Elementem uzupełniającym model strukturalny jest macierz kowariancji wektora zakłóceń stanu Q. Poszczególne elementy tej macierzy określają rozkłady apriori zakłóceń estymowanych wielkości. Interpretacja tej macierzy z punktu widzenia praktyki nawigacyjnej jest następująca elementy jej wyznaczają przedziały ufności, w których mogą znajdować się estymowane parametry nawigacyjne. Na przykład elementy (1,1) i (2,2) macierzy Q wyznaczają przedział myszkowania statku, ściślej mówiąc określają zakłócenia ruchu po szerokości i długości geograficznej. Dla wektora stanu zdefiniowanego wzorem (10) macierz Q może przyjąć postać:, (11), (14) gdzie: gdzie: (12) (13) szerokość geograficzna, długość geograficzna, a duża półoś elipsoidy ziemskiej, e pierwszy mimośród elipsoidy ziemskiej, współczynniki zamiany miary kątowej na liniową na elipsoidzie odniesienia, odpowiednio na południku i równoleżniku. Składowe prędkości średniej i mogą być obliczane jako prędkość wypadkowa z ciągu pozycji zakłócenie ruchu statku po szerokości geograficznej, zakłócenie ruchu statku po długości geograficznej, COG Course Over Ground, SOG Speed Over Ground,, (15), (16), (17) 4
błąd pomiaru COG, błąd pomiaru SOG, błąd określenia poprawki, błąd określenia poprawki. Równania (10) (17) określają model strukturalny procesu nawigacji, gdy estymowanymi wielkościami są współrzędne pozycji, składowe wektora prędkości względem dna oraz poprawki kąta drogi względem dna, prędkości względem dna. Jako wielkości mierzone w modelu pomiarowym przyjmijmy następujące parametry: współrzędne pozycji systemu DGPS, naziemnego systemu radionawigacyjnego, kąt drogi względem dna (COG) i szybkość względem dna (SOG). Elementy wektora pomiarów będą więc następujące:. (18) Macierz pomiarów będzie macierzą Jacobiego, która po obliczeniu poszczególnych pochodnych cząstkowych i odpowiednim uporządkowaniu otrzyma następującą postać:, (19) Uzupełnieniem modelu pomiarowego jest macierz kowariancji zakłóceń pomiarów (wektora pomiarów). Ponieważ niektóre wielkości mierzone nie są ze sobą skorelowane np. pomiary DGPS i radionawigacyjny system naziemny, to macierz ta uprości się. Jeśli przyjmiemy konkretne wartości poszczególnych wariancji i kowariancji występujących w tej macierzy, to otrzymamy:, (20) Model ten został zastosowany w nawigacyjnym systemie stabilizacji pozycji okrętu ratowniczego. W algorytmie i oprogramowaniu przyjęliśmy następujące parametry błędów pomiarów: system DGPS: m, m, współrzędne są nieskorelowane; badania przeprowadzono na Zalewie Szczecińskim i Zatoce Pomorskiej; radionawigacyjny system naziemny AD-2: m, m, m 2 (kowariancja); badania przeprowadzono na Zatoce Gdańskiej; kąt drogi względem dna: ; prędkość względem dna: węzła. Narastanie błędów zliczenia (ich przyrostowy charakter) ilustruje poniższy rysunek (rys. 1). a) 5
b) Rys. 1. Przyrostowy charakter dokładności pozycji zliczonej: a) trajektoria obiektu nawigacji na płaszczyźnie, b) wizualizacja przestrzenna rozkładu normalnego pozycji obiektu. Źródło: opracowanie własne. Jak widzimy, wyraźnie dokładność pozycji zliczonej rozmywa się, co oznacza wzrost elementów macierzy kowariancji pozycji zliczonej (widmo niskoczęstotliwościowe zakłóceń pomiarów). Fakt ten zmusza do stosowania korekcji zewnętrznej pozycji, tj. wykorzystania systemów pozycyjnych, o innym widmie błędów (wysokoczęstotliwościowym). W tym przypadku ewolucja stanu jest określona przez pochodne wyższych rzędów poszczególnych estymowanych parametrów nawigacyjnych. Macierz kowariancji zakłóceń stanu również otrzyma postać dostosowaną do elementów nowego wektora stanu. Tak więc będziemy mieli (macierz diagonalną):, (23) Fuzja pomiarów IS/DR/GPS Innym rozwiązaniem jest sytuacja, gdy wielkościami estymowanymi będą: współrzędne pozycji ( ), rzuty wektora prędkości względem dna na południk i równoleżnik ( ), rzuty wektora przyspieszenia względem dna na południk i równoleżnik ( ) oraz rzuty pochodnych wektora przyspieszenia względem dna na południk i równoleżnik (a, a E ). W przypadku tym wektor stanu będzie posiadał następujące elementy:, (21) Zaś macierz przejścia A będzie określona następująco: macierz jednostkowa, macierz zerowa,, (22) Macierze kowariancji zakłóceń stanu (14) i (23) różnią się tylko tymi elementami, które odpowiadają różnym elementom odpowiadającym im wektorom stanu. W tym modelu za wielkości mierzone przyjmijmy: współrzędne pozycji systemu DGPS ), składowe prędkości względem południka i równoleżnika z nawigacji zliczeniowej (V, V E ), składowe przyspieszenia względem południka i równoleżnika z przetwornika inercjalnego (a, a E ). Przy tych założeniach wektor pomiarów będzie wyglądał następująco: 6
(24) Macierz pomiarów, podobnie jak i wyżej, będzie macierzą Jacobiego. Obliczmy poszczególne pochodne cząstkowe i uporządkujemy. Otrzymamy wówczas następującą bardzo prostą postać macierzy C:, (25) Otrzymaliśmy macierz blokową, co znacznie upraszcza obliczenia i w dużym stopniu zmniejsza błędy numeryczne. Macierz kowariancji zakłóceń pomiarów (wektora pomiarów), po uwzględnieniu braku korelacji oraz dokładności poszczególnych pomiarów, będzie wyglądała następująco: Wykorzystując powyższy model nawigacji zintegrowanej oraz zebrane podczas prób morskich dane z urządzeń nawigacyjnych posłużyły do implementacji w algorytmie obróbki informacji nawigacyjnej. Przeprowadzono również badania symulacyjne. Poniższy rysunek (rys. 2) ukazuje przebieg błędów średnich kołowych poszczególnych pozycji: DR nawigacji zliczeniowej, FK pozycji zintegrowanej po fuzji danych, Hiperbola pozycja obserwowana z naziemnego systemu fazowego Jemiołuszka (hiperbolicznego) oraz DGPS pozycja satelitarna., (26) Rys. 2. Błędy rzeczywiste współrzędnych pozycji w funkcji czasu. Źródło: opracowanie własne. W tym przypadku przyjęliśmy następujące wartości wariancji i kowariancji poszczególnych pomiarów: system DGPS: m, m, współrzędne są nieskorelowane; badania przeprowadzono na Zalewie Szczecińskim i Zatoce Pomorskiej; radionawigacyjny system naziemny AD-2: m, m, m 2 (kowariancja); badania przeprowadzono na Zatoce Gdańskiej; składowych prędkości: ; składowych przyspieszeń: m/s 2. Symulacje wykonano w przedziale jednej minuty. Ze względu na błąd systematyczny występujący w pomiarach logu elektromagnetycznego, błąd pozycji zliczonej wyraźnie narasta wraz z czasem. Po tym czasie wzrasta o 7 metrów (w stosunku do pozycji poprzedniej). W tym czasie błędy rzeczywiste systemów pozycyjnych (Jemiołuszka i DGPS) są porównywalne i wahają się w przedziale od kilkudziesięciu centymetrów do 2 3 metrów (są niezależne od czasu prowadzenia nawigacji). Natomiast średni błąd kołowy pozycji zintegrowanej (FK) jest 2 3 razy mniejszy niż poszczególnych systemów składowych. Ze względu na lepsze uwzględnienie dynamiki statku, model ten ma istotną przewagę nad modelem pierwszym. Okazuje się, że dla prędkości bliskich zeru oraz pracujących sterach aktywnych, log Dopplerowski charakteryzuje się dużymi błędami pomiarowymi. Wtedy koniecznością, pomimo dość wysokiej ceny przetwornika inercjalnego, jest stosowanie modelu INS/GPS [Banachowicz, Bober, 1999]. Wnioski Przedstawione powyżej modele procesu nawigacyjnego nie wyczerpują wszystkich możliwych rozwiązań. W zasadzie można podać przykładów prawie tyle samo, ile będzie stawianych wymagań w stosunku do zbioru estymowanych parametrów nawigacyjnych oraz możliwości pomiarowych. Należy wyraźnie podkreślić, że postać tych modeli decyduje w dużym stop- 7
niu o sukcesie lub porażce opracowywanego filtru. Dotyczy to przede wszystkim poprawnego, adekwatnego do rzeczywistości określenia elementów poszczególnych macierzy przejścia A, pomiarów C oraz macierzy kowariancji Q i R. W przypadku macierzy kowariancji najistotniejszy jest stosunek odpowiednich elementów względem siebie. Przyjęte zbyt duże błędy zakłóceń stanu powodują, że filtr staje się sztywny i za mocno nadąża za pomiarami [Banachowicz, 1995]. Efektem tego jest to, że nie są odfiltrowywane błędy pomiarów. Gdy z kolei przyjmiemy zbyt małe wartości zakłóceń stanu, wtedy filtr zacznie odrzucać pomiary zbyt mocno różniące się od ich prognoz. Szczególnie ważne jest to w rzeczywistych sytuacjach pomiarowych, gdy występuje niezgodność pomiędzy pomiarami z różnych urządzeń i systemów nawigacyjnych oraz przy niskiej wiarygodności mierników oraz wyników pomiarów [Banachowicz, Bober, 1997], [Banachowicz, Bober, 1999]. Nie można również zapominać o problemie synchronizacji skal czasu poszczególnych mierników, długości cykli pomiarowych oraz przedziałach dyskretyzacji. Ponieważ precyzja pomiarów obecnie jest bardzo duża, a prędkości nawigujących obiektów też, to założenie o jednoczesności pomiarów często jest fikcją. Może więc zdarzyć się, że wartość pomiaru będzie dużo różniła się od jego prognozy. Jest to błąd systematyczny skali czasu, po prostu następuje równoległe przesunięcie ciągu pomiarów względem ciągu prognoz. Innym zagadnieniem jest stabilność numeryczna obliczeń. Zapis macierzowo-wektorowy jest bardzo wygodny i dobrze interpretowalny. Gotowe biblioteki procedur i podprogramów również znakomicie ułatwiają tworzenie aplikacji. Jednakże powoduje to rozbudowywanie się algorytmów, co pociąga za sobą kumulowanie się błędów numerycznych i spowolnienie wykonywanych obliczeń. Rozszerzeniem techniki filtracji Kalmanowskiej są filtry cząsteczkowe [Ristic, 2004], w których wykorzystywana jest symulacja Monte Carlo. Filtry te są suboptymalne, ale szczególnie przydatne w przypadkach szybko manewrujących obiektów (silnie nieliniowych równań stanu i pomiarów). Streszczenie W artykule przedstawiono fuzję danych nawigacyjnych uzyskanych z różnych sensorów. Zastosowano rozszerzony filtr Kalmana budując odpowiednie modele stanu i pomiarów procesu nawigacji. Ogólny algorytm filtru Kalmana umożliwia budowanie różnych modeli fuzji danych w nawigacji zintegrowanej. Postać konkretnego modelu jest zdeterminowana możliwościami pomiarowymi parametrów nawigacyjnych oraz założoną postacią wektora stanu. Abstract GPS/IS/DR avigational Data Fusion The article presents multi-sensor navigational data fusion. Extended Kalman s filter was used to build proper models of navigational process (both states and measurements). The general Kalman s filter algorithm allows to build different data model fusions in integrated navigation. The given model form depends on navigational measurements parameters possibilities and established state vector form. Literatura 1. Anderson B.D.O., Moore J.B., Optimal filtering, Prentice-Hall, 1979 New Jersey. 2. Balakrishnan A.V., Kalman Filtering Theory, Optimization Software, 1984 New York. 3. Banachowicz A., Geometria liniowego modelu nawigacji parametrycznej, Zeszyty Naukowe AMW, 1991, 109A. (monografia) 4. Banachowicz A., Wykorzystanie filtru Kalmana w pomiarach skalarnych parametrów nawigacyjnych, Prace Wydziału Nawigacyjnego WSM w Gdyni, 1995, Zeszyt 1, 116 125. 5. Banachowicz A., Bober R., Metodyka obróbki sygnałów z urządzeń nawigacyjnych dla potrzeb system nawigacji zintegrowanej, WSM, 1997 Szczecin. (raport naukowy) 6. Banachowicz A., Bober R., System zintegrowanej nawigacji na okręty ORP Piast i ORP Lech, WSM, 1999 Szczecin. (raport naukowy) 7. Banachowicz A., Variants of Structural and Measurement Models of an Integrated avigational System, Annual of Navigation, 2001, No 3, 5 18. 8. Ristic B., Arulampalm S., Gordon N., Beyond the Kalman Filter. Participle Filters for Tracking Applications, Artech House, 2004 Boston. 8