dr inĪ. Robert GáĊbocki mgr inĪ. Grzegorz ĝwiĊtoĔ
Politechnika Warszawska, Instytut Techniki Lotniczej i Mechaniki Stosowanej
WYBRANE ZAGADNIENIA NAWIGACJI I STEROWANIA
AUTONOMICZNYM POJAZDEM NAZIEMNYM
W artykule autorzy zaprezentowali system autonomicznej nawigacji i sterowania bezzaáogowej platformy naziemnej. CaáoĞü systemu nawigacji skáada siĊ ze zinte-growanego ukáadu INS/GPS wspóádziaáającego z ukáadem nawigacji wizyjnej. Autorzy zaprezentowali zastosowaną metodĊ nawigacji wizyjnej oraz przykáadowe wyniki z badaĔ terenowych.
SOME ASPECTS OF CONTROL AND NAVIGATION SYSTEM FOR AUTONOMOUS UGV
In the paper authors presents system of autonomous navigation and control for UGV. The whole navigation system is the compilation of integrated INS/GPS and visual navigation. Authors present the visual navigation method and some results from field tests of the platform.
1. WPROWADZENIE
Przedstawiane w niniejszym artykule wyniki prac s ą elementem wiekszego projektu, którego celem jest opracowanie autonom icznego system u wykrywania i niszczenia m in niemetalowych i áadunków wybuchowych. Wraz z rozwojem tzw. konfliktów asymetrycznych narasta problem unieszkodliwiania ró Īnego rodzaju m in i áadunków kom binowanych. Ponadto jako pozosta áoĞü po wojnach toczonych w ró Īnych cz ĊĞciach Ğwiata m amy do czynienia z du Īą ilo Ğcią m in, cz Ċsto niem etalowych. S tanowią one du Īe zagro Īenie dla ludnoĞci cywilnej. S ą istotn ą przeszkod ą w powrocie norm alnego Īycia spo áeczno- gospodarczego na tereny, na których wczeĞnie toczyáy siĊ konflikty zbrojne. Z tych powodów grupa badaczy z Politechniki Warszawskiej i Przem ysáowego Instytutu Automatyki i Pomiarów podjĊáa dziaáania w celu opracowania m etod wykrywania i niszczenia takich m in oraz opracowania do tego celu caáoĞciowego autonomicznego mobilnego systemu. Ma on byü umieszczony na m obilnej platform ie z w áasnym nap Ċdem i uk áadem sterowania umoĪliwiającym autonomiczne dziaáanie w terenie. System wykrywania min wyposaĪony jest w urządzenia umoĪliwiające detekcjĊ min i innych áadunków wybuchowych oraz w ukáad ich niszczenia lub neutralizacji. Problem lokalizacji m in niem etalowych jest z technicznego punktu widzenia trudny , poniewa Ī konwencjonalne m etody lokalizacji m in (oparte na wykrywaczach m etalu) s ą w tym przypadku nieprzydatne. W projekcie proponujem y rozwiązanie tego problem u kilkom a sposobam i. WstĊpną lokalizacja m in przeprowadzam y poprzez analizĊ ultradĨwiĊkową lub w podczerwieni. Po wst Ċpnym wykryciu podejrzanego obiektu lokalizacj Ċ m in niem etalowych przeprowadzam y m etodami wykorzystuj ącymi analizĊ zapachow ą przy u Īyciu system u elektronicznego detektora zapachu – „sztucznego nosa” – specjalnie wyczulonego na detekcj Ċ zwi ązków chem icznych wzbudzanych z obudowy miny. Drugi sposób rozpoznania opiera si Ċ na m oĪliwoĞci wykrywania materiaáów
wybuchowych min za pomoc spektroskopii mobilnoĞci jonów (IMS). Niszczenie min odbywa siĊ za pom ocą arm atki bezodrzutowej lub poprzez pozostawienie na m inie áadunku niszczącego. W tym celu wykorzystane są mieszaniny termitowe.
2. SYSTEM STEROWANIA PLATFORMY MOBILNEJ
Rys.1. Ogólny schemat dziaáania przyjĊtego systemu nawigacyjnego
CaáoĞü system u wykrywania i niszczenia m in jest um ieszczona na m obilnej platform ie. W tym celu zaadoptowano elektryczn ą platform Ċ jezdn ą robota pirotechnicznego IBIS. PrzyjĊte m etody wyszukiwania i niszczenia m in wym agają bardzo precyzyjnej nawigacji i sterowania. Dlatego zastosowano ró Īne m etody okre Ğlania po áoĪenia pojazdu oraz jego sterowania w róĪnych etapach jego pracy. Ogólny schemat dziaáania zastosowanego systemu nawigacji przedstawia rys.1. Postanowiono oprze ü ją na zintegrowanym systemie INS/GPS uzupeánionym systemem nawigacji wizyjnej. Wymagania takiego system u determinują pracĊ ukáadu nawigacji i sterowania platformy. W trakcie patrolowania wyznaczonego terenu (trasy) korzystamy gáównie ze wskazaĔ zintegrowanego systemu INS/GPS. Nawigacja wizyjna peáni tylko rol Ċ wspom agającą. Jednak w trakcie podej Ğcia do m iny celem analizy zapachowej a nastĊpnie, w przypadku jej wykrycia, podczas procesu jej neutralizacji lub niszczenia dominującą rolĊ przejmuje nawigacja wizyjna.
Wskazany przez operatora obszar jest przeszukiwany wed áug zadanej trajektorii ruchu pojazdu wyznaczanej za pom ocą uk áadu nawigacji satelitarnej zintegrowanej z uk áadem nawigacji inercjalnej. Wykorzystane zostaną tutaj tak Īe ukáady nawigacji wizyjnej oparte na algorytmach Ğledzących obraz terenu, po którym pojazd si Ċ porusza. Zapewni to precyzyjne przeszukanie ca áoĞci wskazanego obszaru. Obecno Ğü ewentualnych przeszkód terenowych jest wykrywana przez skanowanie terenu oraz jego obserwacj Ċ w Ğwietle widzialnym . Zadania nawigacyjne obejm ują tutaj, oprócz poruszania si Ċ po przewidywanej trajektorii, równieĪ okreĞlenie poáoĪenia miny, precyzyjne wycofanie platformy na bezpieczną odlegáoĞü po pozostawieniu áadunku niszcz ącego lub wycelowanie urz ądzenia niszcz ącego. Celem bezpiecznego wycofania platform y z obszaru zagro Īenia system zapamiĊtuje obraz ostatnich 10 m drogi. Pozwala to na precyzyjne wycofanie si Ċ, przy wykorzystaniu nawigacji wizyjnej, po „w áasnych Ğladach” pojazdu. Generalnie nawigacja wizyjna jest wykorzystywana przez system sterowania w m omentach, gdy wym agana jest jak najwi Ċkasza precyzja ruchu platformy mobilnej.
Rys. 2. Produkowany przez PIAP pojazd IBIS w wersji zadaptowanej do potrzeb opisywanego projektu
Pojazdy takie stanowi ü ma ją cenny elem ent sieciocentrycznego system u zarządzania polem walki. W trakcie prac opracowywana jest niedu Īa gabarytowo platform a m obilna z hybrydowym ukáadem zasilania i koáowym elektrycznym ukáadem jezdnym. Urządzenia do wykrywania i niszczenia min umieszczone bĊdą na owej mobilnej platformie przystosowanej do autonom icznego wykonywania m isji. Platform a jest budowana na bazie opracowanego przez Przemysáowy Instytut Automatyki i Pom iarów robota pirotechnicznego IBIS (rys. 2). W trakcie prac badawczych system nawigacji by á testowany pierwotnie na m aáym pojeĨdzie elektrycznym Mobot Explorer (rys. 3).
System nawigacji wizyjnej zosta á opisany w drugiej cz ĊĞci artykuáu. W pierszej czĊĞci skupiono si Ċ na prezentacji zagadnie Ĕ zwi ązanych z nawigacj ą opart ą na zintegrowanych ukáadach GPS i IMU. System y te wykorzystywane s ą jedynie do nawigacji pojazdu po zadanej trajektorii. W m omencie wykrycia m iny g áówną rol Ċ zaczyna odgrywa ü nawigacja wizyjna jako bardziej dok áadna. Pozwala ona na precyzyjne podej Ğcie do m iny, a nast Ċpnie wycofanie siĊ na bezpieczną odlegáoĞü przed jej zniszczeniem.
Ze wzglĊdu na fakt, i Ī podstawowym zadaniem badawczym projektu jest opracowanie nowatorskich m etod wykrywania m in niem etalowych, w opisie strategii poszukiwania m in pominiĊto potencjalne utrudnienia terenowe. Tym sam ym m oĪliwe jest eksperym entalne sprawdzenie moĪliwoĞci wykorzystania nowatorskich metod nawigacji wizualnej.
Pom iniĊto zagadnienia poruszania siĊ w terenie o powierzchni bardzo „nierównej”, typu rumowiska skalne, lub o du Īej liczbie przeszkód terenowych, np. las. Za áoĪono, Īe pojazd porusza si Ċ po powierzchni poziom ej, wykonuj ąc zadanie wykrywania i niszczenia m in na trasie przygotowywanej dla transportu l ądowego konwojów pojazdów lub grup ludzi, a wiĊc, Īe pojazd porusza si Ċ wzd áuĪ zadanej drogi rozum ianej jako droga, czy przej Ğcie, które pojazd ma za zadanie rozm inowaü. Podsumowując, pojazd porusza si Ċ wzdáuĪ zadanej trasy záoĪonej z odcinków prostoliniowych, lub áuków o staáym promieniu, miĊdzy punktami trasy wprowadzonym i przez u Īytkownika do pam iĊci system u nawigacyjnego pojazdu. MoĪliwe jest tak Īe zaprogram owanie ruchu pojazdu wzd áuĪ równoleg áych, bliskich trajektorii. Jednorazowy przejazd pojazdu wystarcza do rozminowania przejĞcia o wymaganej przez uĪytkownika szerokoĞci. MoĪliwe jest to do uzyskania przez um ieszczenie czujnika na ruchomym ramieniu, co pozwoli na poszerzenie obserwowanego obszaru.
Po wykryciu miny dominującą rolĊ zaczyna odgrywaü nawigacja wizyjna, która nie jest tematem tego artykuáu, wiĊc nie bĊdĊ opisywaá dalszych procedur jego pracy.
3. UKàAD NAWIGACJI INS/GPS
Podstawowym elem entem uk áadu nawigacji platform y jest odbiornik system u nawigacji satelitarnej GPS, a w przysz áoĞci takĪe europejskiego systemu nawigacji satelitarnej Galileo. Zadania pojazdów przeznaczonych do celów bojowych wym agają czĊsto pracy w warunkach „ciszy elektrom agnetycznej”. W terenie zabudowanym sygna á z satelity system u nawigacyjnego moĪe byü niedostĊpny lub obarczony duĪym báĊdem wielotorowoĞci, przez co dokáadnoĞü okre Ğlania w áasnej pozycji platform y m oĪe by ü zbyt m aáa, jak na potrzeby wykonywanej m isji. Jako wspom agający zostanie u Īyty uk áad nawigacji inercjalnej zintegrowany z GPS. Do wspom agania systemu nawigacji wykorzystane zostaną w projekcie takĪe ukáady nawigacji oparte na algorytm ach Ğledzących obraz terenu, po którym pojazd siĊ porusza. W celu uzyskania odpowiedniej jako Ğci nawigacji i sterowania platform ą opracowany zosta á uk áad zintegrowany , wykorzystuj ący wszystkie wym ienione wcze Ğniej
TRIADA PRZYSPESZENIOMIERZY TRIADA GIROSKOPÓW Korekcja wspóáczynnika skali Korekcja wspóáczynnika skali i dryfu G G G y z x , , Korekcja báĊdów zamocowania Macierz transformacji Algorytm obliczania kątów orientacji przestrzennej <,4,) ) , , , (q0 q1q2 q3 QG kor H aG aG G Pole grawitacyjne kor H ȦG ³ ³ G VG G r G WartoĞci początkowe H aG gG G H G C Filtr Kalmana Hf aG kor Hf ȦG GPS ³ VH GHf aG V E N V V V , , G H C Ggps Ggps Ggpsy z x , , Filtr Kalmana gps ) , , (xy z gps V
Rys. 4. Schemat zintegrowanego ukáadu nawigacji INS/GPS
UĪycie do nawigacji po áączenia system ów inercjalnego i nawigacji satelitarnej jest rozwiązaniem od pewnego czasu ju Ī stosowanym w lotnictwie oraz w uk áadach naprowadzania pocisków sterowanych [10, 1 1, 12]. Na tych do Ğwiadczeniach bazowano pierwotnie przy rozpocz Ċciu prac nad projektem oraz inform acjach udost Ċpnianych przez DARPA z wyników konkursów Grand Challange. Uk áady oparte na nawigacji inercjalnej jak INS daj ą pom iar stabilny i niezale Īny od zak áóceĔ zewn Ċtrznych. Problem em jest jednak narastający w czasie b áąd wynikaj ący z procesu ca ákowania wskaza Ĕ przyspieszeniomierzy i giroskopów prĊdkoĞciowych stanowi ących podstaw Ċ pom iaru. Z tego wzgl Ċdu dok áadny pomiar daj ą jedynie przez krótki czas. Z kolei system GPS daje, m niej wi Ċcej, sta áy b áąd niezaleĪny od czasu. Mo Īe on jednak podlega ü skokowym zm ianom. Mog ą wówczas nastĊpowaü zak áócenia zwi ązane ze zm ianami konstelacji obserwowanych satelitów oraz odbiciami sygna áu od budynków lub innych wysokich obiektów . Taki charakter sygna áów uniemoĪliwiaá sterowanie obiektem w sposób ciągáy na podstawie jedynie sygnaáów GPS.
Ukáad INS poáączony jest z odbiornikiem GPS poprzez filtr Kalmana. Pozycja wyliczo-na w tym bloku wykorzystywana jest do aktualizacji warto Ğci pocz ątkowych wykorzystywanych w caákowaniu przyspieszeĔ i prĊdkoĞci w systemie INS. ZaáoĪono, Īe ak-tualizacja pozycji w uk áadzie inercjalnym dokonywana jest gdy b áąd wyliczonej pozycji przekroczy zaáoĪony próg i sygnaá z GPS jest dostĊpny. Bardzo istotną kwestią w procesie in-tegracji jest dobór wspó áczynników filtra Kalmana. W tym celu przeprowadzono seri Ċ badaĔ laboratoryjnych oraz terenowych. Dla porównania przeprowadzono równie Ī próby z ju Ī go-towym zintegrowanym odbiornikiem SPAN. ZdjĊcia z tych prób przedstawia rys. 3.
4. UKàAD NAWIGACJI WIZYJNEJ
Zastosowany ukáad nawigacji wizyjnej korzysta z m etody gradientowej przepáywu optyczne-go do wyznaczania przem ieszczenia obiektu. W ykorzystywane s ą dwa kolejne kadry zarejestrowane przez kamerĊ. Jako wynik dziaáania ukáadu nawigacji otrzymywane są przyro-sty przem ieszczenia x' , 'y pojazdu odpowiednio w kierunku osi Ox i OywyraĪone w lokalnym ukáadzie wspóárzĊdnych kamery.
ZaáoĪenia ukáadu nawigacji wizyjnej
Podczas projektowania ukáadu nawigacji wizyjnej przyjĊte zostaáy poniĪsze zaáoĪenia: x podczaj rejestracji obrazu widoczno Ğü sceny i jej zawarto Ğci nie ro Īni sie zasadniczo
pomiĊdzy kolejnymi kadrami
x znany jest przedziaá czasowy pomiĊdzy rejestrowanymi kadrami
x urządzenie rejestrujące obraz (kamera) jest sztywno przymocowane do pojazdu x Ğwiatáo jest równomiernie rozpraszane na podáoĪu
x rzutowanie równolegáe (bez efektu perspektywy)
x páaszczyzna pola widzenia znajduje sie w staáej odlegáoĞci od kamery. 4.1. Algorytm przepáywu optycznego
Ukáad nawigacyjny wykorzystuje metodĊ przepáywu optycznego Lucasa-Kanade naleĪącą do grupy metod gradientowych przepáywu optycznego.
Równanie przepáywu optycznego dane jest wzorem:
, ,
, ,
I x y t I xGx yGy tGt (1)
gdzie:
I – funkcja intensywnoĞci (jasnoĞci) piksela,
,
x y – wspóárzĊdne poáoĪenia piksela w kadrze,
t – czas,
, ,
x y t
G G G – przyrosty odpowiednio wspóárzĊdnych poáoĪenia oraz czasu.
W metodzie tej zakáada siĊ, Īe przepáyw jest staáy w lokalnym otoczeniu punktu. Tym samym w otoczeniu m mu (m! ) prĊdkoĞü przepáywu 1 V Vx, y jest staáa.
Prawa strona równania (1) moĪe zostaü przedstawiona w formie szeregu Taylora:
, ,
, ,
I I I I x x y y t t I x y t x y t R x y t G G G w G w G w G w w w (2)
gdzie: R– skáadowe wyĪszych rzĊdów szeregu Taylora.
Przy zaáoĪeniu, Īe R i podstawieniu wzoru (2) do wzoru (1) otrzymujemy zaleĪnoĞü: 0 0 I I I x y t xG yG t G w w w w w w (3)
Po zróĪniczkowaniu po czasie równania (3) otrzymujemy: 0 I x I y I t x t y t t t G G G G G G w w w w w w (4)
Po przeksztaáceniu równanie (4) przyjmuje postaü:
x x y y t
I V I V I (5)
Równanie (5) zapisane dla otoczenia m mu punktu
x y, przyjmuje postaü ukáadu 2 n m równaĔ: 1 1 1 2 2 2 : x x y y t x x y y t xn x yn y tn I V I V I I V I V I I V I V I (6)
Który moĪe zostaü zapisany, w formie:
1 1 1 2 2 2 : : : x y t x y x t y xn yn tn I I I I I V I V I I I ª º ª º « »ª º « » « »« » « » « »¬ ¼ « » « » « » « » ¬ ¼ ¬ ¼ (7) oraz: AvG b (8)
PrĊdkoĞci V Vx, y obliczane s ą z wykorzystaniem metody najmniejszych kwadratów
sprowa-dzonej do rozwiązania równania:
T T
A AvG A b (9)
4.2. Wyznaczanie przemieszczeĔ metodą przepáywu optycznego
Schemat dziaáania algorytmu wyznaczania przem ieszczeĔ x' , 'y przedstawiony jest na poniĪszym rysunku.
Rys. 5. Schemat algorytmu wyznaczającego przemieszczenie redukcja rozmiaru kadr z poprzedniego kroku czasu , x y I I t I obliczenia wektora prĊdkoĞci
³
obraz Wspóáczynnik skali
, x y ' ' , x y V V i K 1 i K i K redukcja rozmiaru
Przemieszczenie wzgl Ċdne kam ery wyznaczane jest na podstawie zm iany jasno Ğci pikseli pom iĊdzy dwom a s ąsiednimi kadram i. W kolejnych krokach czasu, uk áad powtarza sekwencjĊ operacji. Zarejestrowany kadr jest zapam iĊtywany w pam iĊci. Nast Ċpnie kadr z chwili czasu i oraz kadr z chwili czasu i są poddawane redukcji rozmiaru. Zredukowany 1
kadr z chwili czasu i sáuĪy jako Ĩródáo danych do obliczenia pochodnych intensywno Ğci pik-seli i ich otoczenia w kierunku osi Ox Oy, . Z kolei zredukowany kadr z chwili czasu i 1
wraz z kadrem z chwili czasu i wykorzystywany jest do obliczenia pochodnej intensywno Ğci pikseli po czasie.
Blok obliczeĔ wektora prĊdkoĞci rozwiązuje równanie (9), a nastĊpnie oblicza Ğrednią wartoĞü ze wszystkich pikseli. Ostatnim etapem jest caákowanie otrzymanych prĊdkoĞci w ce-lu uzyskania przemieszczeĔ ' 'x, y.
5. BADANIA
Ukáad nawigacji wykorzystuje kam erĊ CCD DFK 31BF03. Kam era zainstalowana jest na robocie IBIS. OĞ optyczna kamery jest skierowana w kierunku podáoĪa, prostopadle do niego. Dane (obraz) przesy áane s ą do kom putera z wykorzystaniem przewodu FireW ire (IEEE 1394a). Na rys. 6 przedstawione jest zdjĊcie zainstalowanego systemu.
Rys. 7. Wyniki uzyskane ze ukáadu nawigacyjnego w trakcie autonomicznego przejazdu po wyznaczonej trasie
Jako konkluzj Ċ przeprowadzonych bada Ĕ mo Īna stwierdzi ü, Īe po áączenie system u nawigacji wizyjnej w po áączeniu ze zintegrowanym systemem INS/GPS jest pom ysáem do-brym, dającym zadowalające wyniki. ChociaĪ wymagającym dalszego rozwoju. Przykáadowe wyniki uzyskane w trakcie bada Ĕ terenowych przedstawia rys. 7. Obydwa uk áady nawigacji uzupeániają swoje braki um oĪliwiając prac Ċ pojazdowi w sytuacjach, gdy np. jest problem z prawidáowym sygnaáem GPS. W ówczas rolĊ systemu korygującego wskazania IMU przej-muje nawigacja wizyjna. Nawigacja wizyjna oparta na m etodzie przep áywu optycznego bardzo dobrze uzupeánia inne systemy nawigacyjne. SprawnoĞü algorytmu przetwarzania sy-gnaáu podczas testów potwierdzi áa przydatnoĞü opracowanej metody do okreĞlania poáoĪenia
oraz prĊdkoĞci pojazdu l ądowego. System nawigacji wizyjnej oparty na prostym algorytmie przetwarzania obrazu m oĪe by ü zastosowany na pojazdach ruchom ych autonom icznie lub jako jeden z komponentów zintegrowanego systemu nawigacyjnego.
Prace wykonano w ramach grantu MNiSW nr Nr 0006/R/T00/2008 „Autonomiczny system wykrywania i niszczenia min niemetalowych”.
6. BIBLIOGRAFIA
1. Gader P. D., Pattern recognition for humanitarian de-mining, 16th International
Confe-rence on Pattern Recognition, IEEE, 2002.
2. Bhuyan A., Nath B., Antipersonnel mine detection and classification using GPR image. 18th International Conference on Pattern Recognition 2006 IEEE.
3. El-Rabbany, A., Introduction to GPS: the Global Positioning System., Artech House, Bos-ton, 2002.
4. Hofmann-Wellenhof, B., Lichtenegger, H., Collins, J., Global Positioning System: Theory
and Practice, Springer-Verlag, WiedeĔ, 2001.
5. Kayton, M., Fried, W. R., Avionics Navigation Systems, John Wiley, Nowy York, 1997. 6. Bose, T., Chen, M., Meyer, F., G., Digital Signal and Image Processing, John W iley &
Sons, Hoboken, 2004.
7. Bow, S., Pattern Recognition and Image Preprocessing, W ydanie 2., Marcel Dekker, Nowy York, 2002.
8. Pratt, W. K., Digital Image Processing, Wydanie 3., John W iley and Sons, Nowy York, 2001.
9. Kolodko, J., Vlacic, L., Motion Vision: Design of Compact Motion Sensing Solutions for
Navigation of Autonomous Systems, The Institution of Electrical Engineers, Londyn, 2005.
10. Klotz H., Derbak C., GPS-aided navigation and unaided navigation on the Joint Direct. 11. Ohlmeyer E. J., Guidance, navigation and control without gyros: a gunlaunched
munnition concept. AIAA Guidance, Navigation and Control Conference and Exhibit,
Monterey California 2002.
12. GáĊbocki R, ĩugaj M., Control System for INS Guided Bomb, 47th AIAA Aerospace
Sciences Meeting and Exhibit January 2009.
13. Zasuwa M., G áĊbocki R., ĝwiĊtoĔ G ., Visual navigation for autonomous system for
