Nawigacyjny filtr integrujący GPS/DR

BANACHOWICZ Andrzej 1 PIELA Piotr 2 Nawigacyjny filtr integrujący GPS/DR WSTĘP Współcześnie mamy do czynienia z dynamicznym procesem integracji informacji nawigacyjnej oraz szeroko pojętej globalizacji danych i ich źródeł. Globalny systemu nawigacji jest na tyle uniwersalny że jego praktyczne wykorzystanie jest w zasadzie nieograniczone. Obecnie domeną tego systemu nie jest już wyłącznie nawigacja profesjonalna militarna lub komercyjna ale również wykorzystywany jest do lokalizacji innych obiektów. Należy nadmienić iż w zastosowaniach obronnych bezpieczeństwa wewnętrznego ratowniczych oraz w geodezji geologii leśnictwie i budownictwie określenie dokładnej i wiarygodnej pozycji ma podstawowe znaczenie. Spełnienie tych wymagań jest możliwe dzięki zastosowaniu współczesnych sensorów nawigacyjnych oraz odpowiednich narzędzi informatycznych. Z kolei wysoka precyzja pomiarów i danych nawigacyjnych wymusza stosowanie zaawansowanego aparatu formalnego ich opracowania. Automatyzacja i integracja nawigacji uwarunkowana jest wieloma czynnikami. Do najważniejszych wśród nich można zaliczyć: istniejący poziom techniki i technologii nawigacyjnej wymagania techniczno-eksploatacyjne przesłanki ekonomiczne międzynarodowe i krajowe regulacje prawne dyktowane względami bezpieczeństwa żeglugi ochrony środowiska lub też z innych przyczyn konieczność o której wspomina się rzadko; dotyczy to tych procesów których nie jest w stanie wykonać sam nawigator bo są one na przykład za szybkie lub zawierają zbyt dużo informacji do jednoczesnego przetworzenia a we współczesnej żegludze muszą być wykonane. W precyzyjnej nawigacji wykorzystuje się dane nawigacyjne pochodzące z różnych źródeł: dane aktualne (zazwyczaj pomiarowe) pochodzące z sensorów oraz dane archiwalne pochodzące z publikacji nautycznych i elektronicznych baz danych nawigacyjnych. Fuzja tych niejednorodnych danych oraz informacji o procesie nawigacyjnym (modelu ruchu obiektu nawigującego) realizowane jest w algorytmach zintegrowanych systemów nawigacyjnych oraz ECDIS (Electronic Chart Display and Information System). Powoduje to konieczność łącznego opracowania danych pomiarowych co pozwala na optymalne wykorzystanie informacji nawigacyjnej. Szerzej wielosensorową fuzję danych opisano m.in. w [10] zaś integracje danych GPS z innymi pomiarami nawigacyjnymi w [6] [3] [4] [5]. W prezentowanym artykule przedstawiono jeden z wariantów integracji danych nawigacyjnych uzyskanych z różnych systemów nawigacyjnych. Jako model matematyczny integracji pomiarów zastosowano klasyczny filtr Kalmana. Jego struktura została zdeterminowana dostępnymi urządzeniami nawigacyjnymi układ nawigacji zliczeniowej oraz odbiornik DGPS (Differential Global Positioning System). Przedstawiony algorytm nawigacji zintegrowanej zilustrowano wynikami badań z wykorzystaniem rzeczywistych pomiarów wykonanych podczas prób morskich na okręcie ratowniczym. 1 Zachodniopomorski Uniwersytet Technologiczny w Szczecinie Wydział Informatyki 71-210 Szczecin: ul. Żołnierska 49. Tel: +48 91 449 5517 abanachowicz@wi.zut.edu.pl 2 Zachodniopomorski Uniwersytet Technologiczny w Szczecinie Wydział Informatyki 71-210 Szczecin: ul. Żołnierska 49. Tel: +48 91 449 5584 ppiela@wi.zut.edu.pl 1634

1 FILTR KALMANA Filtracja Kalmanowska jest znana od przeszło pięćdziesięciu lat [8] [9]. Można ją stosować na różnych poziomach obróbki informacji nawigacyjnej poczynając od poziomu pomiarów fizycznych w sensorach (obróbka pierwotna wstępna) poprzez łącznie pomiarów z różnych sensorów (obróbka pośrednia) i kończąc na estymacji współrzędnych pozycji oraz innych parametrów nawigacyjnych obróbka końcowa). Na każdym z tych poziomów posługujemy się identycznym aparatem matematycznym oraz identycznym algorytmem obliczeniowym. Obecnie powszechnie wykorzystujemy urządzenia nawigacyjne o wyjściu cyfrowym (sygnał dyskretny) dlatego też wykorzystujemy algorytm filtru Kalmana dla przypadku dyskretnego losowego układu dynamicznego. System ten w szczególnym przypadku opisuje układ dwóch równań [1] [2] [4] [5]: równanie stanu (model strukturalny) równanie pomiarów (model pomiarowy) (1) (2) gdzie: n-wymiarowy wektor stanu - n-wymiarowy wektor stanu r-wymiarowy wektor zakłóceń stanu m-wymiarowy wektor pomiarów p-wymiarowy wektor zakłóceń pomiarów (utożsamiany z szumem pomiarowym) n n-wymiarowa macierz przejścia m n-wymiarowa macierz pomiarów r n p m. Zakładamy że wektory zakłóceń w i v są szumami gaussowskimi (o rozkładzie normalnym) o zerowym wektorze średnim i są wzajemnie nieskorelowane. W przypadku szumu kolorowego (z trendem) stosuje się tzw. rozszerzony filtr Kalmana EKD (Extended Kalman Filter) gdzie trend zakłóceń jest włączony jako dodatkowe składowe wektora stanu. W powyższych równaniach pominęliśmy macierze przejścia poszczególnych zakłóceń oraz wektor sterowania ze względu na ich nieistotność w prezentowanym modelu nawigacji statku. Równanie stanu opisuje ewolucję systemu dynamicznego opisanego w przestrzeni stanu a model pomiarów łączy funkcyjnie pomiary ze stanem systemu. Rozwiązaniem układu równań (1) (2) przy uwzględnieniu ograniczeń nałożonych na wektory zakłóceń jest filtr Kalmana. Obliczanie wektora stanu w filtrze Kalmana opisuje poniższy algorytm: prognoza wektora stanu (3) gdzie: wartość prognozowana wektora stanu wartość estymowana wektora stanu macierz kowariancji prognozowanego wektora stanu gdzie: Q macierz kowariancji zakłóceń stanu (wektora w) proces innowacji (4) (5) macierz kowariancji procesu innowacji 1635

gdzie: R macierz kowariancji zakłóceń pomiarów (wektora v) macierz wzmocnienia filtru (macierz Kalmana) (6) wartość estymowana wektora stanu z filtracji po wykonaniu pomiaru macierz kowariancji estymowanego wektora stanu 2 STRUKTURA FILTRU INTEGRUJĄCEGO (7) (8) O postaci przyjętego w algorytmie układu dynamicznego decyduje sytuacja pomiarowa tj. zestaw urządzeń nawigacyjnych których pomiary można wykorzystać oraz model nawigacji na który składają się parametry nawigacyjne wymagające estymacji na podstawie pomiarów. W tym przypadku zarejestrowano pomiary współrzędnych pozycji z systemu DGPS pomiary wektora prędkości z logu Dopplerowskiego oraz kursu z kompasu żyroskopowego. Ze względu na dostępność urządzeń nawigacyjnych oraz rodzaj zarejestrowanych pomiarów wykorzystanych do praktycznej weryfikacji algorytmu przyjęto następujące składowe wektora stanu: współrzędne pozycji (długość geograficzna szerokość geograficzna na elipsoidzie WGS-84 rzuty wektora prędkości względem dna na równoleżnik i południk ( ) kąt drogi względem dna oraz szybkość względem dna ( ). W tym przypadku wektor stanu przyjmie postać: (9). (10) Modelem strukturalnym systemu jest równanie stanu (wzór 1). Stąd macierz przejścia A będzie następującej postaci: (11) gdzie: (12) 1636

(13) szerokość geograficzna długość geograficzna a duża półoś elipsoidy ziemskiej e pierwszy mimośród elipsoidy ziemskiej i wektory prędkości średniej odpowiednio po równoleżniku i po południku współczynniki zamiany miary kątowej na liniową na elipsoidzie odniesienia odpowiednio na południku i równoleżniku zależne od szerokości geograficznej pozycji statku i parametrów elipsoidy odniesienia w tym przypadku WGS-84. Wektory i mogą być obliczane jako prędkość wypadkowa z ciągu pozycji systemu radionawigacyjnego. Innym elementem modelu strukturalnego jest macierz kowariancji wektora zakłóceń stanu Q. Jej elementy określają rozkłady a priori 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. Przykładowo elementy diagonalne (11) i (22) macierzy Q określają zakłócenia ruchu po szerokości i długości geograficznej będące efektem myszkowania statku lub niedokładności określenia jego położenia. Dla wektora stanu zdefiniowanego wzorem (10) macierz zakłóceń stanu Q może przyjąć postać: (14) gdzie: zakłócenie ruchu statku po szerokości geograficznej ( myszkowanie statku) zakłócenie ruchu statku po długości geograficznej ( myszkowanie statku) COG Course Over Ground SOG Speed Over Ground błąd pomiaru COG błąd pomiaru SOG. (15) (16) (17) 1637

Model strukturalny statku jako układu dynamicznego określają równania (10) (17) przy założeniu że estymowanymi wielkościami są: współrzędne pozycji składowe wektora prędkości względem dna oraz kąta drogi i prędkości względem dna. Wielkościami mierzonymi (model pomiarowy) są następujące parametry: współrzędne pozycji pochodzące z systemu DGPS rzuty wektora prędkości względem dna na równoleżnik i południk ( ) kąt drogi względem dna (COG) i szybkość względem dna (SOG). Stąd wektor pomiary przyjmie postać:. (18) Macierz pomiarów jest macierzą Jacobiego która ma następującą postać: (19) Elementem modelu pomiarowego jest również macierz kowariancji zakłóceń pomiarów (wektora pomiarów). Jest to macierz wstęgowa ponieważ niektóre wielkości mierzone nie są ze sobą skorelowane np. pomiary DGPS i składowe prędkości z nawigacji zliczeniowej podobnie pomiary żyrokompasowe i z logu. (20) 1638

3 ZAŁOŻENIA D WERYFIKACJA FILTRA INTEGRUJĄCEGO Podany powyżej model procesu nawigacji został zaimplementowany w środowisku Matlab przetestowany na danych numerycznych oraz zweryfikowany z wykorzystaniem rzeczywistych pomiarów wykonanych na okręcie ratowniczym podczas prób morskich [3]. Zarejestrowana seria zawierała 517 pomiarów i obejmowała ruch prostoliniowy oraz wykonanie pełnej cyrkulacji i wyjście z cyrkulacji do kursu prostego. Wektor pomiarów obejmował: szerokość i długość geograficzną (na elipsoidzie WGS-84) rejestrowaną na wyjściu 12-kanałowego odbiornika DGPS DSM Pro firmy TRIMBLE (stacja różnicowa ROZEWIE) częstotliwość rejestracji 1 Hz; kurs żyrokompasowy z żyrokompasu PLATH firmy PLATH częstotliwość rejestracji 8 Hz; prędkość względem dna (składowa wzdłużna i składowa poprzeczna) z logu hydroakustycznego DOLOG-20 firmy KRUPP ATLAS ELECTRONIC częstotliwość rejestracji 1 Hz. W algorytmie przyjęto następujące założenia dotyczące dokładności pomiarów: system DGPS: m m współrzędne są nieskorelowane; kąt drogi względem dna: ; prędkość względem dna: węzła. Dla tych danych elementy macierze kowariancji będą miały następujące wartości numeryczne: macierz kowariancji zakłóceń stanu macierz kowariancji zakłóceń pomiarów. Początkowa macierz kowariancji wektora stanu z założenia była następująca:. 4 WYNIKI Wyniki na wyjściu filtra przy rzeczywistych danych wejściowych zostały przedstawione na rysunkach 1 4. Pełną trajektorię statku ukazuje rysunek 1. obejmujący wszystkie pomiary. Ze względu na skalę rysunku poszczególne pozycje statku (zmierzona z DGPS prognozowana w filtrze oraz estymowana na wyjściu filtra) pokrywają się. 1639

Rys. 1. Trajektoria statku. [opr. własne] Natomiast na rysunku 2. widzimy już istotne różnice pomiędzy tymi pozycjami (zobrazowanie akwenu to kwadrat o boku 10 m). Rys. 2. Fragment trajektorii statku. [opr. własne] Przyjmując pozycję estymowaną (najdokładniejszą spośród trzech wyżej wymienionych) jako wartości poprawne (odniesienia) obliczono odchylenia pozycji zmierzonej z DGPS od wzorcowej i wyniki przedstawiono na rysunku 3. Odchylenia te nie przekraczały 15 m (oprócz 3 przypadków na 517). Jest to skutkiem dużej dokładności systemu DGPS. 1640

Rys. 3. Odchylenia pozycji DGPS od pozycji estymowanej. [opr. własne] Analogicznie obliczono błędu pozostałych estymowanych parametrów nawigacyjnych: błąd kołowy pozycji błąd szerokości geograficznej błąd długości geograficznej kowariancja pomiędzy szerokością i długością geograficzną współczynnik korelacji pomiędzy szerokością i długością geograficzną Jego mała wartość potwierdza wyniki wcześniejszych badań [3] oraz przyjęte założenia do weryfikacji numerycznej algorytmu. błąd składowej południkowej prędkości statku błąd składowej równoleżnikowej prędkości statku błąd kąta drogi statku względem dna 1641

błąd szybkości statku względem dna Wykres na rysunku 4. ilustruje powierzchnię Gaussa funkcji gęstości prawdopodobieństwa pozycji DGPS.. Rys. 4. Funkcja gęstości prawdopodobieństwa błędów pomiarów współrzędnych pozycji z DGPS. [opr. własne] PODSUMOWANIE Przedstawiony w artykule model i algorytm ilustruje jedną z wielu możliwości nawigacyjnego wykorzystania filtru Kalmana do integracji danych nawigacyjnych. W modelu tym przyjęto wektor pomiarów zgodny z danymi uzyskanymi podczas prób morskich [3]. Odpowiednio do tego opracowano wektor stanu w którym ewolucję systemu (trend) odtworzono na podstawie nawigacji zliczeniowej. Głównymi zaletami filtru Kalmana w tym przypadku jest jego rekurencyjność co jest naturalną koniecznością w przypadku prowadzenia nawigacji oraz możliwość wykorzystania informacji o ruchu obiektu (jego trendzie). Przedstawione badania z wykorzystaniem rzeczywistych dynamicznych pomiarów nawigacyjnych dowodzą że filtracją Kalmanowską jest dobrą metodą estymacji w zastosowaniach nawigacyjnych a zastosowany algorytm był adekwatny do sytuacji pomiarowej oraz dynamiki statku. Przyjęte założenia co do rozkładów błędów pomiarowych oraz zakłóceń stanu w model również nie wykazały istotnych odchyleń w stosunku do wartości estymowanych na wyjściu filtra. Są też inne podejścia do filtracji Kalmanowskiej opierające się na symulacji Monte Carlo [7] i metodach sztucznej inteligencji [11] które umożliwiają identyfikację parametrów modelu stanu i pomiarów on-line. Streszczenie We współczesnej precyzyjnej nawigacji wykorzystuje się dane nawigacyjne pochodzące z różnych źródeł: dane aktualne uzyskane z sensorów oraz dane archiwalne otrzymywane z publikacji nautycznych i elektronicznych baz danych nawigacyjnych. Fuzja tych niejednorodnych danych oraz informacji o procesie nawigacyjnym (modelu ruchu obiektu nawigującego) realizowana jest w algorytmach zintegrowanych systemów nawigacyjnych oraz ECDIS (Electronic Chart Display and Information System). W prezentowanym artykule przedstawiono jeden z wariantów integracji danych nawigacyjnych uzyskanych z systemów nawigacyjnych o różnej zasadzie pomiarowej. Jako model matematyczny integracji pomiarów zastosowano klasyczny filtr Kalmana. Jego struktura została zdeterminowana dostępnymi urządzeniami nawigacyjnymi układ nawigacji zliczeniowej oraz odbiornik DGPS (Differential Global Positioning System). Przedstawiony algorytm nawigacji zintegrowanej zilustrowano wynikami badań z wykorzystaniem rzeczywistych pomiarów 1642

wykonanych podczas prób morskich na okręcie ratowniczym. Navigation integrated filter GPS/DR Abstract Nowadays precise navigation uses navigation data from various sources: current data from sensors and archival data from the nautical publications and the electronic navigation database. The fusion of these heterogeneous data and information about the navigation process (the model of motion of the navigating object) is performed in the integrated algorithms of navigation systems and in the Electronic Chart Display and Information System (ECDIS). The article presents a variant of the integration of navigational data obtained from the navigation systems of different measurement principle. As the mathematical model of the integration of measurements the classical Kalman filter is used. Its structure was determined by the available navigation devices the dead reckoning navigation system and DGPS ( Differential Global Positioning System). The presented algorithm of integrated navigation is illustrated by results of research of studies using actual measurements taken during sea trials the ship rescue. BIBLIOGRAFIA 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. Bober R. System zintegrowanej nawigacji na okręty ORP Piast i ORP Lech WSM 1999 Szczecin. (raport naukowy) 4. Banachowicz A. Variants of Structural and Measurement Models of an Integrated Navigational System Annual of Navigation 2001 No 3 pages 5 18. 5. Banachowicz A. Banachowicz G. Fuzja pomiarów nawigacyjnych GPS/INS/DR. Logistyka nr 6/2013. Poznań 2013 ss. 2 8 (CD). 6. Farrell J.A. Barth M. The Global Positioning System & Inertial Navigation. McGrew-Hill New York 1999. 7. Julier S.J. Uhlmann J.K. Durrant-Whyte H.F. A New Approach for Filtering Nonlinear Systems. The Proceedings of the American Control Conference. Seatlle Washington pages 1628 1632 1995. 8. Kalman R.E. A New Approach to Linear Filtering and Prediction Problems.ASME Journal of Basic Engineering series D 82 1960. 9. Kalman R.E. Bucy R.S. A New Approach to Linear Filtering and Prediction Theory.ASME Journal of Basic Engineering series D 83 1961. 10. Mitchell H.B. Multi-Sensor Data Fusion. An Introduction. Springer Verlag Berlin Heidelberg 2007. 11. Ristic B. Arulampalm S. Gordon N. Beyond the Kalman Filter. Participle Filters for Tracking Applications Artech House 2004 Boston. 1643