Streszczenie 6. Rozdział 1. Wstęp Obszar problemowy Cel i zakres pracy... 9

Spis treści Streszczenie 6 Rozdział 1. Wstęp 7 1.1. Obszar problemowy............................... 7 1.2. Cel i zakres pracy................................ 9 Rozdział 2. Systemy sensoryczne robotów mobilnych 12 2.1. Niepewność pomiarów przetwarzanych w systemie percepcyjnym robota mobilnego..................................... 12 2.2. Odometria.................................... 15 2.2.1. Zasada i fizyczne podstawy pomiaru.................. 15 2.2.2. Odometria robota Labmate....................... 17 2.2.3. Model niepewności odometrii..................... 22 2.2.4. Podsumowanie............................. 27 2.3. Dalmierze ultradźwiękowe........................... 29 2.3.1. Budowa i fizyczne podstawy działania................. 29 2.3.2. Charakterystyka dalmierza Polaroid.................. 32 2.3.3. Podsumowanie............................. 41 2.4. Skanery laserowe................................ 42 2.4.1. Budowa i fizyczne podstawy działania................. 42 2.4.2. Charakterystyka skanera zbudowanego w KARiI........... 46 2.4.3. Charakterystyka skanera laserowego Sick LMS 200.......... 63 2.4.4. Podsumowanie............................. 75 2.5. Sensory wizyjne................................. 77 2.5.1. Sensory wizyjne w robotach mobilnych................ 77 2.5.2. Sposób analizy i modelowania niepewności sensorów wizyjnych... 80 2.5.3. Kamery monitorujące.......................... 81 2.5.4. Pozycjonowanie za pomocą kamery pokładowej i landmarków.... 87 Rozdział 3. Przetwarzanie i gromadzenie danych uzyskanych z sensorów 92 3.1. Reprezentacje informacji przestrzennej w systemie nawigacji robota..... 92 3.1.1. Sposoby reprezentacji modelu otoczenia................ 92 3.1.2. Przetwarzanie informacji niepewnej w metodach budowy modelu otoczenia................................ 94 3.2. Mapy rastrowe................................. 96 3.2.1. Metoda probabilistyczna........................ 96 3.2.2. Metoda oparta na teorii ewidencji................... 102 3

3.2.3. Metoda oparta na zbiorach rozmytych................. 107 3.2.4. Wyniki eksperymentów i porównanie metod............. 110 3.3. Mapa wektorowa................................ 116 3.3.1. Podstawowe założenia i struktura mapy................ 116 3.3.2. Akwizycja i wstępne przetwarzanie danych ze skanera laserowego.. 117 3.3.3. Grupowanie pomiarów......................... 118 3.3.4. Segmentacja.............................. 122 3.3.5. Estymacja parametrów cech geometrycznych oraz ich niepewności. 126 3.3.6. Tworzenie mapy globalnej przy znanej pozycji robota........ 132 3.4. Modele hybrydowe i konwersja reprezentacji................. 135 3.4.1. Reprezentacja hybrydowa w przetwarzaniu danych z sensorów odległości......................... 135 3.4.2. Mapa rastrowa jako reprezentacja pośrednia.............. 136 3.4.3. Konwersja mapy rastrowej na reprezentację wektorową........ 137 3.4.4. Tworzenie map wektorowych ze wsparciem lokalnej reprezentacji rastrowej................................. 143 Rozdział 4. Autonomiczna nawigacja na podstawie danych uzyskanych z sensorów 148 4.1. Zagadnienie samolokalizacji robota mobilnego................ 148 4.2. Jednoczesna samolokalizacja i tworzenie mapy otoczenia........... 150 4.2.1. Zagadnienie SLAM........................... 150 4.2.2. Jednoczesna samolokalizacja i tworzenie mapy z użyciem EKF... 153 4.3. Realizacja algorytmu EKF-SLAM....................... 161 4.3.1. Reprezentacja mapy wektorowej.................... 161 4.3.2. Ekstrakcja cech............................. 165 4.3.3. Wykorzystanie reprezentacji SPmap w algorytmie EKF-SLAM... 166 4.3.4. Wyniki eksperymentów........................ 169 4.3.5. Wnioski................................. 177 4.4. Zastosowanie sensorów logicznych w systemie SLAM............ 178 4.4.1. Koncepcja modyfikacji systemu EKF-SLAM............. 178 4.4.2. Ekstrakcja cech geometrycznych ze wsparciem reprezentacji rastrowej 181 4.4.3. Estymacja przemieszczenia robota................... 182 4.4.4. Wyniki eksperymentów........................ 188 4.4.5. Wnioski................................. 194 Rozdział 5. Architektura rozproszonego systemu robotów i sensorów stacjonarnych 196 5.1. Wprowadzenie................................. 196 5.2. Wieloagentowy system robotów i sensorów stacjonarnych............................ 198 5.3. Architektura agenta robota........................... 201 5.3.1. Analiza procesu przetwarzania informacji przez robota mobilnego......................... 201 5.3.2. Proponowana struktura tablicy i zestaw agentów........... 203 5.3.3. Realizacja EKF-SLAM we współpracy z innymi robotami...... 206 5.4. Architektura agenta percepcyjnego....................... 207 4

5.5. Struktura systemu komunikacji między agentami............... 210 5.5.1. Protokół negocjacji między agentami................. 210 5.5.2. Mechanizm komunikacji między agentami.............. 212 5.6. Badania eksperymentalne............................ 213 5.6.1. Pozycjonowanie z użyciem sensorów wizyjnych........... 213 5.6.2. Współdziałanie podczas tworzenia mapy otoczenia.......... 215 5.7. Wnioski..................................... 218 Rozdział 6. Planowanie działań robota z uwzględnieniem niepewności percepcji 221 6.1. Wprowadzenie................................. 221 6.2. Model grafowy przestrzeni akcji........................ 224 6.3. Metody planowania sekwencji akcji...................... 226 6.4. Eksperymentalna weryfikacja metody planowania akcji............ 229 6.5. Wnioski..................................... 232 Rozdział 7. Podsumowanie i wnioski 233 Literatura 239 Abstract 253

Streszczenie W rozprawie przeanalizowano procesy percepcji i modelowania świata w systemie nawigacji autonomicznego robota mobilnego oraz przedstawiono nowe metody i algorytmy przetwarzania danych na poszczególnych etapach tych procesów. Uwagę skupiono na systemach percepcji, w których wykorzystuje się sensory odległości, a w szczególności skanery laserowe. Określono źródła niepewności pomiarów dokonywanych wybranymi sensorami robota mobilnego i dla każdego z tych sensorów podano model niepewności. Przedstawiono analizę propagacji niepewności w systemie percepcyjnym robota. Przeanalizowano wybrane metody przetwarzania informacji i gromadzenia wiedzy o środowisku robota pod względem możliwości reprezentacji i propagacji niepewności. Opracowano nowe oraz ulepszone metody przetwarzania takiej informacji w systemie nawigacji robota, cechujące się zwiększoną odpornością na niepewność i niekompletność danych wejściowych. Zaproponowano nową strukturę systemu modelowania świata robota, opartą na sensorach logicznych przetwarzających dane uzyskane ze skanera laserowego oraz na różnych, współistniejących reprezentacjach informacji przestrzennej. Proponowane metody redukcji niepewności w systemie nawigacji robota mobilnego uzupełniono o metodę percepcji aktywnej algorytm planowania akcji pozycjonujących, który pozwala robotowi podejmować działania redukujące niepewność jego pozycji. Przedstawione w rozprawie metody i algorytmy zostały zweryfikowane w badaniach eksperymentalnych z wykorzystaniem rzeczywistych autonomicznych robotów mobilnych i reprezentatywnych systemów sensorycznych.

Rozdział 1 Wstęp 1.1. Obszar problemowy Do najszybciej rozwijających się badań w dziedzinie robotyki należą badania dotyczące autonomicznych robotów mobilnych. Jest to obszar bogaty w problemy badawcze i atrakcyjny ze względu na swoją interdyscyplinarność (robot mobilny integruje osiągnięcia wielu gałęzi nauk technicznych) oraz nietrywialny charakter napotykanych w nim zagadnień. Szczególną cechą problemów rozwiązywanych podczas budowy i programowania robotów autonomicznych jest ich złożoność koncepcyjna i algorytmiczna oraz jednoczesna konieczności znajdowania rozwiązań działających w czasie rzeczywistym w oparciu o niepewne, niejednoznaczne i niekompletne dane wejściowe. Obecnie, z jednej strony, roboty autonomiczne stanowią jeden z istotniejszych obszarów weryfikowania metod sztucznej inteligencji (SI), a z drugiej strony, budowa robotów jest uważana przez wielu badaczy za właściwy sposób osiągnięcia postępu w dziedzinie SI [167]. Roboty mobilne są atrakcyjne nie tylko z poznawczego punktu widzenia, znajdują coraz więcej zastosowań praktycznych. W przemyśle od dawna wykorzystywane są zautomatyzowane pojazdy transportowe, określane skrótem AGV (ang. Automated Guided Vehicle) [135, 222]. Obecnie, dwa główne obszary perspektywicznych zastosowań autonomicznych robotów mobilnych sytuują się jednak poza przemysłem. Są to usługi oraz dość szeroko pojęta sfera ochrony i bezpieczeństwa. W pierwszej z tych grup można wyróżnić wiele typów usług, które mogą być świadczone przez roboty o różnym stopniu komplikacji i autonomii. Znane są zastosowania robotów jako przewodników podczas wystaw lub targów [167]. Ten rodzaj aplikacji rozwinął się obecnie do postaci systemów wielorobotowych, zdolnych do nieprzerwanego, samodzielnego działania przez dłuższy czas [24]. Produkowane są także autonomiczne systemy sprzątające przeznaczone do obiektów wielkopowierzchniowych, a proste roboty odkurzacze dostępne są już w handlu. Do usługowych należy też zaliczyć rozwijane od kilku lat roboty mobilne pełniące funkcję indywidualnych opiekunów osób niepełnosprawnych lub w podeszłym wieku. Dynamicznie rozwijającą się dziedziną zastosowań robotów mobilnych jest zapewnianie bezpieczeństwa. Do znanych już policyjnych robotów inspekcyjnych, interwencyjnych i pirotechnicznych [201] oraz robotów strażników [105] dołączyły roboty ratownicze i poszukiwawcze, charakteryzujące się znacznie większą autonomią. Także tutaj myśli się 7

o zwiększeniu efektywności działania przez zastosowanie zespołów współpracujących robotów [118, 233]. Robot mobilny jest rodzajem robota, którego podstawową funkcją jest przemieszczanie się względem otoczenia (lokomocja), a układ wykonawczy tego robota jest układem lokomocyjnym. Roboty mobilne mogą się znacznie różnić budową układu lokomocyjnego w zależności od środowiska, w którym działają i stawianych przed nimi zadań. Przedstawione w rozprawie rozważania ograniczono do robotów poruszających się po lądzie. W robotach takich stosowane są najczęściej kołowe [116] lub kroczące [332] układy lokomocyjne. Autonomiczny robot mobilny wykonuje swoje zadanie bez zewnętrznego wsparcia ze strony człowieka. Podstawową właściwością takiego robota jest zdolność do samodzielnego tworzenia i wykonywania planów działania na podstawie obserwacji otoczenia. W niniejszej pracy przez pojęcie robota autonomicznego rozumie się autonomicznego robota mobilnego. Autonomiczny robot mobilny powinien być zdolny do samodzielnej nawigacji. Nawigacja (łac. navigatio żegluga) jest umiejętnością określenia własnej pozycji i kursu (lub drogi) do celu na podstawie mapy. W przypadku robota mobilnego realizacja zadania nawigacji polega na: budowie modelu otoczenia, określaniu swojej pozycji względem otoczenia, planowaniu i monitorowaniu wykonania sekwencji ruchów prowadzących do osiągnięcia celu. Część systemu sterowania robota mobilnego odpowiedzialna za te funkcje nazywana jest systemem nawigacji. System nawigacji można rozpatrywać jako złożony układ sterowania ze sprzężeniem zwrotnym względem stanu otoczenia i/lub od modelu tego otoczenia. System ten pobiera dane z otoczenia za pomocą sensorów (czujników) i oddziałuje na to otoczenie za pomocą efektorów układu wykonawczego (lokomocyjnego). Polecenia ruchu wypracowane w systemie nawigacji wykonywane są przez podrzędne wobec niego warstwy systemu sterowania robota, zapewniające wypełnianie takich funkcji, jak śledzenie trajektorii i sterowanie poszczególnymi serwomechanizmami układu lokomocyjnego. Zagadnienia sterowania robotami mobilnymi nienależące do zadań systemu nawigacji nie będą poruszane w niniejszej rozprawie. W piśmiennictwie polskim zagadnieniom tym poświęcono liczne publikacje, między innymi monografie [116, 303]. Badania dotyczące robotów autonomicznych prowadzone są w licznych ośrodkach od końca lat sześćdziesiątych ubiegłego wieku. W tym czasie wykształciły się dwa główne podejścia do budowy systemów sterowania robotów autonomicznych deliberatywne (zwane także hierarchicznym) oraz behawioralne [18]. Podejście deliberatywne opiera się na koncepcji racjonalnego rozumowania (deliberacji). Koncepcję tę wykorzystano w metodach planowania działań opartych na klasycznych algorytmach SI [224]. Systemy deliberatywne realizowane są w postaci łańcucha: percepcja planowanie działanie, w którym sensory dostarczają danych do budowy wewnętrznego modelu świata robota, pełniącego centralną funkcję w systemie nawigacji. Podejście behawioralne neguje natomiast rolę wewnętrznego modelu świata. Sygnały z sensorów wykorzystywane są do równoległego wypracowywania wielu prostych zachowań robota, których interakcje mają prowadzić do inteligentnego zachowania całego systemu [56]. Ponieważ oba te, w znacznym stopniu przeciwstawne, podejścia do problemu sterowania robotem autonomicznym w formie czystej nie spełniły oczekiwań, rozpoczęto poszukiwania hybrydowych struktur systemu sterowania łączących zalety obu roz- 8

wiązań, czyli umiejętność wyboru optymalnej sekwencji działań (właściwość systemów deliberatywnych) oraz zdolność reakcji w czasie rzeczywistym na bodźce ze środowiska (charakterystyczną dla systemów behawioralnych). Paradygmat hybrydowy jest obecnie standardem w projektowaniu systemów sterowania robotów autonomicznych. Proponuje się wiele architektur oprogramowania robota opartych na tym podejściu, a przegląd reprezentatywnych rozwiązań można znaleźć w pracach [167, 217]. Niezależnie od przyjętej struktury systemu sterowania (a więc także systemu nawigacji), warunkiem koniecznym uzyskania autonomii w rozumieniu podanej wyżej definicji robota autonomicznego jest skuteczna percepcja cech otoczenia i prawidłowa interpretacja obserwacji za pomocą procedur decyzyjnych sterujących robotem. Przez percepcję rozumie się tu całokształt działań podejmowanych w celu identyfikacji wybranych parametrów modelu obserwowanego obiektu lub procesu. Są to działania zmierzające do przypisania dokonanym przez dany czujnik robota pomiarom wielkości charakteryzujących środowisko określonej interpretacji w kategoriach zrozumiałych dla systemu sterowania tego robota. Główną przeszkodą skutecznej i niezawodnej percepcji jest niepewność i niekompletność charakteryzująca dane uzyskiwane z sensorów robota. Z punktu widzenia podstawowych zadań systemu nawigacji najistotniejsze jest zapewnienie skutecznej percepcji na potrzeby procedur służących do budowy modelu otoczenia oraz określania pozycji robota. Budowa mapy i określanie pozycji są ze sobą ściśle powiązane i dopiero pomyślne rozwiązanie obu tych zagadnień pozwala robotowi mobilnemu utworzyć pełny model świata, złożony z mapy otoczenia i stanu samego robota 1. Wchodzące także w skład systemu nawigacji procedury planowania działań i ruchów (planer ścieżki i trajektorii) nie odwołują się zazwyczaj bezpośrednio do danych uzyskiwanych z sensorów, lecz do modelu świata [98, 186], nawet wówczas, gdy planowanie ma charakter dynamiczny [240]. Warstwa reaktywna systemu sterowania robota nie korzysta z modelu świata, lecz reaguje na sygnały z sensorów, które nie są poddawane złożonej obróbce [18]. W związku z tym reaktywne procedury sterowania robotem, także wtedy, gdy stanowią część architektury hybrydowej, wykazują się zazwyczaj większą odpornością na niepewność danych uzyskiwanych z sensorów [278, 280]. 1.2. Cel i zakres pracy Celem niniejszej rozprawy jest systematyczna analiza procesów percepcji i modelowania świata w systemie nawigacji robota autonomicznego oraz opracowanie metod i algorytmów redukcji niepewności informacji na poszczególnych etapach tych procesów. W kontekście rozważanego zagadnienia nawigacji autonomicznego robota mobilnego za kluczowy problem można uznać budowę modelu świata, obejmującego stan (pozycję) robota oraz mapę otoczenia. Ważkość zagadnienia modelowania świata robota potwierdzają także liczne publikacje dotyczące tej tematyki. Na podstawie ich analizy można jednak stwierdzić, że w niewielu pracach porusza się temat wpływu niepewności danych wejściowych na skuteczność percepcji, modelowania świata i nawigacji robotów autonomicznych. Z ważniejszych publikacji zawierających głębszą analizę wpływu niepewności pomiarów na działanie poszczególnych komponentów systemu nawigacji robota mobilnego wymienić należy monografie Adamsa [4] oraz Castellanosa i Tardósa [72]. Problemy dotyczące niepewności pomiarów poruszono także w dalszych pracach Tardósa i współpracowników [73, 74], pracach 1 Wartości prędkości i przyśpieszeń (dynamiczne składowe stanu) nie są zazwyczaj określane. 9

Kleemana [89, 161] oraz dysertacjach Arrasa [23] i Jensfelta [141]. W pracach tych autorzy skupiają się na pojedynczym, wybranym zadaniu nawigacji (głównie określaniu pozycji robota) wykonywanym z użyciem jednego rodzaju sensora [4, 141]. Jedynie w pracach [23, 72] opisano układy wielosensoryczne, obejmujące skaner laserowy i kamery wizyjne. Z kolei w pracach ujmujących modele sensorów i systemów wielosensorycznych w ogólniejszym kontekście [99, 189] pomija się wiele zagadnień charakterystycznych dla robotów mobilnych, z których najważniejsza jest adekwatność modelu obserwowanego obiektu do rzeczywistej sytuacji na scenie działania robota. Brak jest w światowym piśmiennictwie jednorodnej metodologicznie analizy niepewności pomiarów różnych, współpracujących sensorów robotów mobilnych, a także zaleceń dotyczących projektowania systemów percepcji i modelowania świata robota mobilnego w sposób odporny na niepewność danych wejściowych. Analiza i metody redukcji niepewności percepcji w systemie nawigacji robota mobilnego są więc stosunkowo słabo zbadane. Główne tezy rozprawy są następujące: 1. Istotna z punktu widzenia modelowania świata robota niepewność pomiarów dokonywanych sensorami ma charakter jakościowy i jest wynikiem nieadekwatności przyjętego modelu obserwowanego obiektu lub procesu do rzeczywistej sytuacji na scenie. Niepewność ta może być zidentyfikowana i częściowo zredukowana. 2. Identyfikacja przyczyn niepewności pomiarów o charakterze jakościowym umożliwia opracowanie metod interpretacji danych uzyskanych z sensorów charakteryzujących się zwiększoną odpornością na ten rodzaj niepewności. 3. Hybrydowy model otoczenia robota, złożony z reprezentacji parametrycznej (wektorowej) i nieparametrycznej (rastrowej), pozwala na pełniejsze uwzględnienie niepewności informacji. 4. Wprowadzenie do systemu dynamicznego modelowania świata robota dodatkowych informacji o niepewności wynikającej z błędnej identyfikacji obserwowanych obiektów oraz informacji umożliwiających lepszą predykcję stanu (opartą na rozszerzonym filtrze Kalmana) poprawia jakość budowanego modelu środowiska. 5. Analityczny model procesu percepcji i propagacji niepewności pomiarów w zadaniu wyznaczania pozycji umożliwia syntezę takiej procedury planowania optymalnej sekwencji akcji pozycjonujących (percepcji aktywnej), która gwarantuje pożądaną jakość estymaty pozycji robota. Prawdziwość powyższych tez wykazano głównie na podstawie badań eksperymentalnych z wykorzystaniem rzeczywistych autonomicznych robotów mobilnych i reprezentatywnych systemów sensorycznych. W rozprawie poruszono takie zagadnienia, jak: ocena i modelowanie niepewności pomiarów wybranych rodzajów sensorów robota mobilnego oraz analiza propagacji niepewności pomiarów w systemie percepcyjnym robota; analiza wybranych metod przetwarzania informacji i gromadzenia wiedzy o środowisku robota pod względem ich zdolności do reprezentacji i propagacji niepewności pomiarów oraz opracowanie nowych lub ulepszonych metod przetwarzania takiej informacji, cechujących się zwiększoną odpornością na niepewność danych wejściowych; 10

analiza wpływu niepewności pomiarów przetwarzanych w systemie percepcyjnym na proces budowy wewnętrznego modelu świata robota oraz opracowanie metod pozwalających zredukować ten wpływ dzięki wykorzystaniu idei percepcji wielosensorycznej i aktywnej. Rozważania dotyczące powyższych zagadnień zostały ujęte w siedmiu rozdziałach. W rozdziale 2 zaproponowano metodykę badania sensorów robotów mobilnych i sposób opisu niepewności pomiarów, adekwatny do systemu nawigacji robota. Dokonano identyfikacji modelu niepewności pomiaru wykonywanego przez wybrane sensory odległości i podsystem odometrii robota TRC Labmate. Przedstawiono także analizę procesu propagacji niepewności pomiarów wykonywanych kamerami wizyjnymi wykorzystywanymi jako pokładowe lub stacjonarne sensory do pozycjonowaniu robota. Rozdział 3 zawiera przegląd i analizę wybranych metod przetwarzania i gromadzenia danych z sensorów robota. Zwrócono uwagę na problem reprezentacji i propagacji różnych rodzajów niepewności pomiarów oraz na redukcję niepewności informacji w systemach wielosensorycznych. Przedstawiono nowe i ulepszone algorytmy: tworzenia mapy rastrowej, szybkiego grupowania pomiarów i odpornej estymacji parametrów prostej. Zaproponowano także oryginalną koncepcję wyodrębniania (ekstrakcji) cech geometrycznych ze wsparciem reprezentacji rastrowej. Rozdział 4 poświęcono zagadnieniu jednoczesnej samolokalizacji i tworzenia mapy otoczenia. Zidentyfikowano podstawowe źródła niepewności informacji związane z wyodrębnianiem cech złożonych środowisk oraz z predykcją stanu robota na podstawie pomiarów odometrycznych. W celu rozwiązania występujących tu problemów zaproponowano nową strukturę systemu modelowania świata robota, opartą na sensorach logicznych [128] przetwarzających dane uzyskane ze skanera laserowego. W rozdziale 5 zaproponowano architekturę systemu nawigacji robota mobilnego oraz architekturę zespołu robotów i sensorów stacjonarnych. Przedstawiono protokół negocjacji pozwalający robotom wymieniać dane wspierające nawigację z uwzględnieniem niepewności tych danych. W rozdziale 6 zaprezentowano metodę planowania sekwencji akcji pozycjonujących podejmowanych przez robota mobilnego korzystającego z kamery pokładowej i stacjonarnych kamer monitorujących. Metoda ta pozwala robotowi podejmować optymalne działania redukujące niepewność jego pozycji. Rozdział 7 jest podsumowaniem całości rozprawy. Zestawiono w nim osiągnięte rezultaty oraz zarysowano kierunki dalszych badań. Niniejsza rozprawa jest wynikiem badań prowadzonych przez autora w ciągu kilku lat w Laboratorium Robotów Mobilnych Instytutu Automatyki i Inżynierii Informatycznej (IAiII) Politechniki Poznańskiej. Duży wpływ na ostateczny kształt tematyki tych badań miały doświadczenia zdobyte przez autora podczas udziału w projekcie COPERNICUS nr ER- BIC 15CT Multi-Agent Robot System for Industrial Applications in the Transport Domain oraz kierowania projektem badawczym KBN nr 8T11A 0019 Sieci percepcyjne w robotyce [264]. Rezultaty tego ostatniego projektu zostały bezpośrednio wykorzystane w przedstawionych w rozprawie koncepcjach architektury oprogramowania robota i systemu wielorobotowego.

Rozdział 2 Systemy sensoryczne robotów mobilnych 2.1. Niepewność pomiarów przetwarzanych w systemie percepcyjnym robota mobilnego Pomiary wielkości fizycznych, a szczególnie geometrycznych, pełnią bardzo istotną funkcję w systemach sterowania robotów autonomicznych. Bez użycia sensorów nie jest możliwe rozpoznanie stanu środowiska działania robota i podejmowanie decyzji w zależności od tego stanu. W konstrukcji robotów autonomicznych wykorzystywane są bardzo różne sensory z uwagi na różnorodność zadań stawianych robotom oraz zróżnicowanie i złożoność środowisk, w których działają. Sensory stosowane obecnie w robotach mobilnych można podzielić na następujące grupy [193]: Sensory wewnętrzne odwołują się do stanów wewnętrznych robota, takich jak przyśpieszenie czy kąt obrotu koła. Do tej grupy należą sensory przemieszczenia i położenia (potencjometry pomiarowe, rezolwery, przetworniki obrotowo-impulsowe, tarcze kodowe), kompasy, żyroskopy, inklinometry, sensory sił i momentów w przegubach. W kołowych robotach mobilnych zliczanie obrotów kół wykorzystywane jest do określania pozycji. Kompasy i żyroskopy służą do określania orientacji robota względem zewnętrznego układu współrzędnych [32, 52]. Sensory mierzące siły i momenty oraz inklinometry używane są głównie w robotach kroczących [332]. Sensory zewnętrzne służą do identyfikacji wielkości zewnętrznych w stosunku do samego robota, a będących istotnymi cechami obserwowanego otoczenia (np. odległość od wybranych obiektów, cechy fotometryczne tych obiektów, ich wzajemne położenie itp.). Sensory zewnętrzne można podzielić na kilka grup, wymienionych tu w kolejności odpowiadającej wzrastającemu znaczeniu informacji dostarczanych robotowi: dotykowe (zderzaki, czujniki nacisku), zbliżeniowe (optyczne, pojemnościowe), odległości (dalmierze: laserowe, ultradźwiękowe), obrazu (kamery wizyjne). 12

Spośród wymienionych grup pierwsza i druga obejmują sensory wykorzystywane w robotach mobilnych przede wszystkim jako zabezpieczenia antykolizyjne [104]. Natomiast dalmierze i sensory wizyjne stanowią główne źródło informacji dla systemu nawigacji robota autonomicznego. Sensory zadaniowe służą do wykonywania wybranych funkcji użytkowych przez robota, ale nie dostarczają informacji niezbędnych do funkcjonowania jego systemu sterowania. Do tej grupy można zaliczyć czujniki ruchu [104] oraz czujniki wykrywające ogień, skażenie promieniotwórcze lub określone substancje chemiczne (gazy, materiały wybuchowe) [201]. Stosowanie sensorów zadaniowych ściśle zależy od danej aplikacji robota mobilnego. Nie będą one w niniejszej pracy rozpatrywane. W dalszej części tego rozdziału podjęto próbę systematycznego opisu właściwości kilku grup sensorów, skupiając się na analizie i ocenie niepewności pomiarów oraz proponując modele niepewności pomiaru dla wybranych sensorów. Przez model niepewności pomiaru rozumiany jest opis (model) matematyczny umożliwiający ocenę niepewności danych uzyskanych z pomiaru i predykcję niepewności pomiarów w znanym środowisku. Opis dotyczy wybranych sensorów zewnętrznych dalmierzy laserowych i ultradźwiękowych oraz sensorów wewnętrznych wykorzystywanych w podsystemie odometrii. Omówiono konkretne typy sensorów stosowanych w Laboratorium Robotów Mobilnych IAiII, są one typowe dla swoich klas sensory podobne lub takie same (Sick LMS 200, Polaroid) są używane w wielu robotach mobilnych na świecie. W opisie kamer wizyjnych, które są znaną od dawna i dobrze przebadaną klasą sensorów, skupiono się na ocenie niepewności pomiaru w konkretnych zastosowaniach, zgodnie z zaproponowaną poniżej metodyką. Problemy związane z interpretacją przez roboty autonomiczne informacji pochodzącej z sensorów czynią percepcję zadaniem bardzo złożonym. Wiedza o niepewności pomiarów jest niezbędna w systemie sterowania robota. Warunkuje ona przede wszystkim możliwość wnioskowania o przydatności poszczególnych pomiarów do określonych celów, np. omijania przeszkód, samolokalizacji, budowy modelu otoczenia. Należy zwrócić uwagę, że ten sam pomiar w zależności od niepewności, którą jest obarczony, może być wykorzystany jedynie do określonych celów. Na przykład, dokładne pomiary odległości do kilku obiektów mogą posłużyć do uaktualnienia położenia i orientacji robota, ale jeżeli ich niepewność (wyrażona odchyleniem standardowym) jest duża, to pomiary te będą przydatne jedynie jako informacja o wystąpieniu przeszkód, które robot musi ominąć. Prawidłowa ocena niepewności poszczególnych pomiarów jest warunkiem koniecznym ich prawidłowego i efektywnego łączenia w struktury wyższego poziomu [99], stanowiące element modelu otoczenia robota. W szczególności, ocena niepewności przestrzennej (tzn. niepewności co do położenia i/lub orientacji w danym układzie współrzędnych) charakterystycznych cech otoczenia zaobserwowanych przez sensory jest niezbędna do prawidłowego przypisania wyników pomiarów do modeli określonych obiektów lub zjawisk w tym otoczeniu występujących, czyli prawidłowego kojarzenia danych (ang. data association). Problem ten jest fundamentalny dla zagadnień percepcji i nawigacji, nie tylko dla robotów mobilnych [30]. Podstawowym problemem jest brak ogólnej metody analizy różnego rodzaju błędów związanych z pomiarami za pomocą sensorów robotów. Klasyczne podejście do analizy błędów pomiarów zakłada ich podział na błędy przypadkowe, systematyczne i nadmierne (pomyłki) [296]. Ten tradycyjny podział opiera się na założeniu, że błędy przypadkowe powodowane są przez nieznane czynniki losowe, natomiast błędy systematyczne mają swe źródło w czynnikach zdeterminowanych, które należy zidentyfikować i wyeliminować lub skompenso- 13

wać ich wpływ (wprowadzić poprawki). Tak pojmowane błędy przypadkowe można wykryć na podstawie serii pomiarów, ponieważ zmieniają się one w czasie co do wartości i znaku, powodując rozrzut wartości zmierzonych, który wyrażany jest zazwyczaj przez odchylenie standardowe σ. Błędy systematyczne pozostają stałe podczas wykonywania serii pomiarów w niezmiennych warunkach, stąd ich ujawnienie jest trudniejsze i zazwyczaj wymaga wykonania dodatkowych, odpowiednio zaplanowanych eksperymentów [315]. Stosowany w metrologii do analizy błędów aparat rachunku prawdopodobieństwa i statystyki matematycznej jest dobrze znany [87], a zasady postępowania i sposób opisu niepewności pomiarów są ujęte w odpowiednich normatywach i zaleceniach [297]. Specyfika pomiarów dokonywanych za pomocą sensorów robotów autonomicznych sprawia, że ten aparat pojęciowy staje się do opisu ich niepewności niewystarczający. Jest to spowodowane kilkoma grupami czynników. Autonomiczne roboty mobilne działają w zmieniającym się środowisku. Nawet przyjmując, że środowisko jest statyczne, czyli że nie ma w nim ruchomych (dynamicznych) obiektów, nie można założyć istnienia znanego i niezmiennego układu warunków fizycznych wpływających na pomiary. Sam ruch robota mobilnego w otoczeniu powoduje, że o ile można założyć np. stałą i znaną temperaturę powietrza, o tyle nie da się z góry przewidzieć właściwości fizycznych napotykanych przeszkód czy intensywności oświetlenia. Roboty działają w czasie rzeczywistym. Ograniczenia czasowe narzucone systemowi sterowania robota często wykluczają wielokrotne powtarzanie pomiarów, co zmusza do wnioskowania o niepewności na podstawie małolicznej próby lub nawet pojedynczego pomiaru. Pomiary z użyciem sensorów robotów są zawsze pomiarami pośrednimi. Mierzona wielkość fizyczna (napięcie, czas, przesunięcie fazy) jest często powiązana z ostatecznym wynikiem pomiaru (odległość, kąt) i jego niepewnością w sposób bardzo złożony, a otrzymanie wyniku wymaga wieloetapowego przetwarzania, zarówno analogowego jak i cyfrowego (programowego). Ostateczne wyniki procesu pomiarowego są często wielkościami wielowymiarowymi (położenie punktu na płaszczyźnie lub w przestrzeni, parametry opisujące odcinek itp.), co wymusza bardziej skomplikowany niż w pomiarach wielkości skalarnych opis niepewności. Powyższa lista najważniejszych problemów wskazuje, że klasyczne metody i narzędzia oceny niepewności wyniku pomiarów nie są wystarczające w przypadku sensorów robotów autonomicznych. Pod względem metodologicznym i pojęciowym bardziej przydatny wydaje się podział niepewności proponowany w pracy [315], gdzie wyróżnia się niepewność typu A, wyznaczoną metodami statystycznymi, oraz niepewność typu B, wyznaczoną innymi metodami, lecz ocenianą także za pomocą odchyleń standardowych (lub wariancji). Proces pomiaru jest tu rozumiany jako identyfikacja wybranych parametrów modelu matematycznego danego obiektu lub procesu. Specyfika procesów pomiarowych w robotach autonomicznych powoduje, że znaczna część rozważań w dalszej części niniejszego rozdziału będzie dotyczyła niepewności typu B. W sytuacjach, w których nie ma możliwości wykonania wielokrotnych pomiarów i określenia niepewności typu A przez wyznaczenie odchylenia standardowego, niepewność typu B będzie określana przez odwołanie się do modeli zjawisk fizycznych leżących u podstaw pomiaru danym sensorem i podanie wielkości traktowanych 14

dalej jako oceny odchyleń standardowych wynikających z niepewności wprowadzanej przez dane zjawisko. Charakterystyczną cechą sensorów robotów autonomicznych jest także brak jednoznaczności pomiarów, tzn. brak możliwości ustalenia na podstawie samego pomiaru (a nawet serii pomiarów), czy nie jest on całkowicie błędny, gdyż np. odnosi się do innego obiektu niż ten, który był przedmiotem pomiaru. Niepewność co do przedmiotu pomiaru nie jest rozważana w publikacjach z zakresu metrologii [296, 315] lub statystycznej analizy pomiarów [92], jest natomiast przedmiotem zainteresowania badaczy zajmujących się matematycznymi podstawami śledzenia obiektów, np. w radiolokacji [30] oraz nawigacji morskiej i lotniczej [31]. Niepewność co do przedmiotu pomiaru nie może być analizowana tymi samymi metodami co pozostałe dwa rodzaje niepewności. Do badania tej niepewności wykorzystywane są narzędzia specyficzne dla danej aplikacji, zazwyczaj porównujące i łączące pomiary wykonane w różnych miejscach i/lub w pewnym odstępie czasu. W związku ze złożoną naturą niepewności pomiarów dokonywanych sensorami robotów autonomicznych w niniejszej pracy proponuje się sposób postępowania zmierzający do uzyskania jak najpełniejszego modelu niepewności, użytecznego z punktu widzenia systemu nawigacji robota. Na poszczególnych etapach tego postępowania należy: określić fizyczne podstawy działania danego typu sensora i zidentyfikować zjawiska mające wpływ na poszczególne rodzaje niepewności pomiarów; określić użyteczny zakres pomiarowy danego sensora i stwierdzić, jakie zjawiska i stany otoczenia (np. rodzaje obserwowanych obiektów) są przyczyną niepewności co do przedmiotu pomiaru; w stałym układzie warunków fizycznych wykonać eksperymenty identyfikujące przyczyny rozrzutu pomiarów i wyznaczyć metodami statystycznymi odchylenia standardowe tych pomiarów; dla pozostałych zjawisk i/lub parametrów sensora wpływających na niepewność pomiarów określić odchylenie standardowe innymi metodami; zbudować model matematyczny procesu pomiarowego uwzględniający źródła niepewności pomiarów typu A i typu B, a następnie określić wpływ niepewności obu typów na niepewność wektora parametrów będącego wynikiem pomiaru. 2.2. Odometria 2.2.1. Zasada i fizyczne podstawy pomiaru Koncepcja nawigacji zliczeniowej Istotnym wymaganiem stawianym autonomicznym robotom mobilnym jest zdolność do samolokalizacji, czyli określania swojego położenia i orientacji względem wybranego układu odniesienia (por. p. 4.1). Najprostszym rozwiązaniem jest tutaj wyznaczenie bieżącej pozycji danego pojazdu (robota mobilnego) na podstawie poprzedniej pozycji, znanego kursu i prędkości oraz czasu przemieszczania. Procedura taka nosi w literaturze anglojęzycznej nazwę dead reckoning, wywodzącą się od żeglarskiego określenia deduced reckoning, oznaczającego obliczenie 15

v położenia statku według kursu i logu [293]. W większości robotów mobilnych procedura ta przyjmuje formę samolokalizacji zliczeniowej, gdyż sumuje się (zlicza) elementarne przemieszczenia pojazdu, i nosi nazwę odometrii. W przypadku robotów kołowych techniczne rozwiązanie odometrii jest stosunkowo proste, gdyż wykorzystuje się pomiar elementarnych przemieszczeń kątowych kół. Sposób realizacji odometrii zależy od zastosowanych układów pomiarowych, a postać równań opisujących zliczanie elementarnych przemieszczeń od kinematyki układu jezdnego danego robota. W przyrządzie służącym do pomiaru przebytego dystansu, tzw. odometrze (hodometrze, drogomierzu), najczęściej wykorzystuje się przetworniki fotoelektryczne (zwane potocznie enkoderami) umieszczone na osiach kół robota lub bezpośrednio na osiach silników. Wyróżnić można inkrementalne (przyrostowe) przetworniki obrotowo-impulsowe, mierzące przemieszczenie kątowe, oraz tarcze kodowe, mierzące położenie kątowe. Enkodery inkrementalne są prostsze i tańsze niż tarcze kodowe [132]. Są one stosowane w większości kołowych robotów mobilnych [104]. Kinematyka robota o układzie jezdnym typu różnicowego Pozycję kołowych robotów mobilnych poruszających się w dwuwymiarowej, płaskiej przestrzeni roboczej R 2 określa się zazwyczaj za pomocą wektora parametrów x R = [x R y R θ R ] T, z których x R,y R określają położenie, a θ R orientację robota w kartezjańskim układzie odniesienia, względem którego odbywa się ruch robota. Wektor x R jest traktowany jako wektor stanu robota. y v y R(k) R(k) a y R(k-1) y R(k-1) 2r n l r c rot b x x R(k-1) x R(k) x Rysunek 2.1. Robot mobilny poruszający się po ścieżce o stałym zakrzywieniu Wszystkie roboty używane w Laboratorium Robotów Mobilnych IAiII mają układ dwukołowy ze sterowaniem różnicowym (ang. differential drive), najczęściej spotykany układ wśród badawczych robotów mobilnych. Robot taki jest ciałem sztywnym wyposażonym 16

w dwa koła o osiach prostopadłych do jego osi symetrii, napędzane niezależnie. Sterowanie ruchem robota odbywa się przez różnicowanie prędkości obrotu kół napędowych. Środek (domyślnej) osi między kołami jest punktem, wokół którego obraca się pojazd, jednocześnie jest to punkt charakterystyczny, w którym zaczepiony jest lokalny układ współrzędnych robota. Na potrzeby analizy kinematyki przyjęto założenie, że robot porusza się bez poślizgu. Oznacza to, że prędkości chwilowe kół w punkcie styku z podłożem są równe zeru, a wektor prędkości postępowej robota pokrywa się z osią jego korpusu. By spełnić warunek toczenia się obu kół bez poślizgu, robot musi się poruszać po ścieżce o stałym zakrzywieniu, zataczając łuk o promieniu a wokół chwilowego środka obrotu c rot (rys. 2.1). Punkt c rot leży na linii będącej przedłużeniem osi kół napędowych robota. Ponieważ w każdej chwili oba koła robota poruszają się względem punktu c rot z tą samą prędkością obrotową ω, można zapisać warunki określające ich prędkości postępowe v l (koło lewe) i v r (koło prawe) względem układu odniesienia: v l = ω ( a + b 2 ), v r = ω ( a b ). (2.1) 2 Biorąc pod uwagę powyższy warunek oraz geometrię i wymiary pojazdu, można wyznaczyć zależność między prędkością obrotową ω i postępową v robota a prędkościami postępowymi kół: ω = v r v l, v = v r + v l, (2.2) b 2 gdzie b jest rozstawem (bazą) kół. Prędkość obrotowa i postępowa robota są funkcjami czasu, ulegają zmianie pod wpływem zmieniających się chwilowych wartości v l i v r (sterowanie ruchem robota), spełniając zawsze warunek (2.2). Równania stanu kinematycznego robota w zewnętrznym układzie odniesienia przyjmują postać: ẋ R = v(t)cos θ R (t), ẏ R = v(t)sin θ R (t), θr = ω(t). (2.3) Znając początkowy stan robota mobilnego x R(0) = [x 0 y 0 θ 0 ] T w pewnej chwili t 0 i sygnały sterujące v l (t), v r (t), można wyznaczyć, biorąc pod uwagę wzory (2.3) oraz (2.2), położenie i orientację robota w chwili t k = t 0 + δt: x R = tk t 0 v(τ)cos θ R dτ, y R = tk t 0 v(τ)sin θ R dτ, θ R = tk t 0 ω(τ)dτ. (2.4) Równania (2.4) i (2.2) opisują kinematykę prostą robota mobilnego o układzie jezdnym typu różnicowego [97]. Jednocześnie określają one ogólną procedurę samolokalizacji zliczeniowej dla tego rodzaju pojazdu. 2.2.2. Odometria robota Labmate Realizacja odometrii Robot TRC Labmate ma układ jezdny typu różnicowego. Ruch odbywa się dzięki parze silników prądu stałego, umieszczonych centralnie wzdłuż osi przebiegającej przez środek geometryczny korpusu. Napęd jest przenoszony na koła o kauczukowym bieżniku i średnicy 150 17

mm. Robot ma 4 samonastawne koła podporowe. Pojazd jest wyposażony w sterownik mikroprocesorowy wykonujący podstawowe funkcje sterowania silnikami i wypełniający rozkazy przesyłane z komputera nadrzędnego [312]. Położenie i orientacja robota są wyznaczane w sterowniku na zasadzie samolokalizacji zliczeniowej (wzory 2.4), w której wykorzystuje się dane z czujników przemieszczenia kątowego kół. W przypadku robota Labmate są to impulsy z przetworników obrotowo-impulsowych umieszczonych na osiach silnika lewego i prawego. Impulsy te są przeliczane na elementarne przemieszczenia kątowe koła, odpowiednio α l oraz α r, z uwzględnieniem rozdzielczości przetwornika. Przyrost drogi (elementarne przemieszczenie liniowe) dla lewego d l i prawego d r koła wyrażony jest odpowiednio przez d l = α l r l, d r = α r r r, (2.5) gdzie r l = r r = r n = 75 mm i jest nominalnym promieniem koła napędowego. Gdy samolokalizacja odbywa się przez zliczanie elementarnych przyrostów drogi w bardzo krótkich odcinkach czasu, można uznać, że prędkości kół podczas ruchu są stałe. Przy założeniu, zgodnie z przyjętym modelem kinematycznym (2.4), że robot porusza się po ścieżce o stałym zakrzywieniu (rys. 2.1), przyrost drogi całego pojazdu d i jednostkową zmianę orientacji θ wyrażają wzory: d = d r + d l, (2.6) 2 θ = d r d l, (2.7) b gdzie b = 340 mm i jest bazą kół robota. Zmiana położenia środka robota w kartezjańskim układzie współrzędnych wyznaczana jest z zależności geometrycznych: ( x = d cos θ R(k 1) + θ ) (, y = d sin θ R(k 1) + θ ). (2.8) 2 2 Nowa orientacja robota w chwili k obliczana jest na podstawie θ i znanej orientacji w chwili poprzedniej: θ R(k) = θ R(k 1) + θ, (2.9) a nowe położenie środka robota w chwili k obliczane jest na podstawie równań: Przyczyny błędów w odometrii x R(k) = x R(k 1) + x, y R(k) = y R(k 1) + y. (2.10) Odometria oparta jest na założeniu, że przemieszczenia kątowe kół jednoznacznie determinują przemieszczenie (liniowe i kątowe) całego pojazdu względem podłoża. Niespełnienie tego podstawowego założenia jest główną przyczyną błędów w określeniu pozycji robota metodą odometryczną. Część błędów ma charakter systematyczny i wynika z przybliżonego schematu obliczeniowego odometrii (podanego w poprzednim podrozdziale), w którym wykorzystuje się model idealnych kół o punktowym styku z podłożem oraz równej średnicy. Błędy systematyczne pojawiają się podczas każdego powtórzenia danego eksperymentu, jednak w zależności od warunków tego eksperymentu (np. kształtu ścieżki lub rozkładu ładunku) mogą ujawnić 18

się tylko niektóre z nich. Błędy odometrii zależą też od wielu czynników losowych, których rozkłady prawdopodobieństwa nie są znane, a powodem występowania są zazwyczaj interakcje między pojazdem (kołami) a konkretnym rodzajem podłoża lub obiektami w otoczeniu robota. Klasyfikacja błędów odometrii robotów o różnicowym układzie jezdnym została zaproponowana w pracach Borensteina i współpracowników [51, 53, 106]. Zgodnie z tą propozycją, błędy dzieli się na błędy systematyczne, powodowane przez: rzeczywistą wartość promienia koła, różną od przyjętej wartości r n, rzeczywisty rozstaw kół, różny od przyjętej wartości bazy b, różnice w średnicy kół napędowych (np. wskutek zużycia ogumienia), asymetrię zamocowania kół lub skrzywienie ich osi względem korpusu, ograniczoną rozdzielczość enkoderów, oraz błędy przypadkowe, których najczęstszymi przyczynami są: poślizgi kół, powodowane między innymi przez: śliskie podłoże, nadmierne przyśpieszenia, siły zewnętrzne (np. kolizje) oraz siły wewnętrzne (np. pochodzące od oporów kół podporowych [106]), niepunktowy kontakt kół z podłożem, nierówności powierzchni, po której porusza się robot, przetoczenie się koła przez niespodziewaną przeszkodę na podłożu. Jeżeli robot porusza się po płaskiej, równej powierzchni ze stosunkowo niedużą prędkością, to prawdopodobieństwo wystąpienia błędów przypadkowych jest niewielkie, a dominującą przyczyną niepewności pozycji robota stają się błędy systematyczne. Pogląd ten jest podzielany przez wielu badaczy [51, 68, 104, 198] na podstawie doświadczeń z różnymi robotami mobilnymi. Niekiedy błędy powodowane przez kolizje z przeszkodami (siły zewnętrzne) lub toczenie się kół po bardzo nierównym podłożu (przejazd przez próg, przewody na podłodze itp.) określane są mianem katastrofalnych [52], gdyż często powodują gwałtowne odchylenie estymowanej przez odometrię pozycji robota od pozycji prawdziwej. Dla robota o różnicowym układzie jezdnym szczególnie niebezpieczne są błędy powodujące znaczne, nieprawidłowe naliczenie przyrostu drogi jednego z kół, np. w wyniku przetoczenia się tego koła przez przeszkodę. Powoduje to bowiem duży błąd estymowanej orientacji robota, który, jak nietrudno zauważyć, analizując równania kinematyki (2.4), wpływa na błąd położenia w kolejnych chwilach. Zgodnie z metodyką analizy niepewności, zaproponowaną w podrozdziale 2.1, błędy katastrofalne można uznać za niepewność co do przedmiotu pomiaru, która powinna być eliminowana na poziomie systemu sterowania robota, np. dzięki łączeniu (fuzji) danych z różnych sensorów [52]. Jak wynika z przytoczonej powyżej klasyfikacji, źródłem błędów systematycznych jest sama konstrukcja robota. Większość błędów jest wynikiem geometrycznej niedokładności w wykonaniu lub montażu struktury mechanicznej układu jezdnego. Błędy systematyczne nie zależą od interakcji robota ze środowiskiem, mogą więc być usunięte lub ograniczone przez odpowiednią kalibrację. 19

W przeciwieństwie do błędów systematycznych błędy przypadkowe odometrii wynikają głównie z interakcji układu jezdnego robota i otoczenia. Interakcje te powodują naruszenie założenia o ruchu bez poślizgu oraz warunków (2.1) i (2.2) wiążących prędkości postępowe kół z prędkością obrotową i postępową robota. Błędów przypadkowych nie można całkowicie wyeliminować, chociaż niektóre konfiguracje jezdne robotów mobilnych (np. układ synchro drive [106]) wykazują większą odporność na nie niż rozważany tu układ różnicowy, którego największą wadą jest bezpośrednie przekładanie się każdego poślizgu koła na błąd orientacji pojazdu. Badanie odometrii w kontrolowanym środowisku Określenie rzeczywistych wartości wielkości geometrycznych, od których zależą błędy systematyczne odometrii, jest trudne, ponieważ: użytkownik nie ma możliwości precyzyjnego zmierzenia niektórych wielkości bez demontażu robota, efektywne średnice kół i baza b zależą od zużycia i ugięcia ogumienia, w związku z czym powinny być określane podczas ruchu z właściwym ładunkiem (akumulatory, sensory, komputer pokładowy itp.). Efektywna, a przy tym stosunkowo prosta procedura szacowania całkowitych błędów odometrii oraz kalibracji błędów systematycznych jest przedstawiona w pracy [51]. Nosi ona nazwę UMBmark (ang. University of Michigan Benchmark), a jej część doświadczalna polega na n-krotnym przebyciu przez robota mobilnego ścieżki w kształcie kwadratu o boku l umb (w pracy [51] l umb = 4 m) i zanotowaniu dla każdego przejazdu odchylenia niezależnie zmierzonych współrzędnych kartezjańskich punktu, w którym zatrzymał się robot, od współrzędnych odczytanych z odometrii (przy bezbłędnej odometrii robot powinien powrócić do punktu startu). Procedura zbierania danych jest prowadzona dwukrotnie: dla kierunku jazdy zgodnego z ruchem wskazówek zegara i przeciwnego do tego ruchu. Na podstawie tych pomiarów wyznacza się współczynniki określające stosunek rzeczywistej do nominalnej długości bazy kół robota E b oraz stosunek promieni kół napędowych E d : E b = ˆb b n, E d = r r r l, (2.11) gdzie b n jest długością nominalną bazy, a ˆb jej estymowaną długością. Poniżej przedstawiono sposób wyznaczenia E b i E d oraz współczynników korygujących, które będą użyte w procedurze odometrii niezależnie od sterownika robota. Wyprowadzenie poniższych wzorów z zależności geometrycznych opisujących ścieżkę testu UMBmark można znaleźć w pracy [51]. Punktem wyjścia są środki ciężkości wyznaczone dla grup punktów z przejazdów w kierunku zgodnym ze wskazówkami zegara i przeciwnym, odpowiednio x ecw, ȳ ecw i x eccw, ȳ eccw. Na ich podstawie wyznacza się kąty: α e odpowiadający średniemu błędowi obrotu w miejscu podczas wykonywania testu oraz β e określający błąd orientacji robota na końcach prostoliniowych części jego ścieżki: 20

α e = 1 ( xecw + x eccw 2 4l umb β e = 1 ( xecw x eccw 2 4l umb + ȳe cw ȳ e ccw 4l umb + ȳe cw + ȳ e ccw 4l umb ), (2.12) ). (2.13) Następnie oblicza się współczynniki E b i E d : E b = π π α e, E d = ( ) l umb + bsin βe 2 l umb bsin ( βe 2 ) (2.14) oraz skorygowane wartości wielkości geometrycznych podlegających kalibracji: ˆb = Eb b n, ˆr l = 2 E d + 1 r n, ˆr r = 2E d E d + 1 r n. (2.15) W procedurze korekcji błędów systematycznych przedstawionej w pracy [51] założono, że średnia wartość promienia obu kół napędowych r = rr+r l 2 odpowiada wartości nominalnej promienia r n. Jeżeli jednak średni promień kół odbiega od wartości nominalnej, to pojawia się błąd systematyczny, który nie jest korygowany w teście UMBmark. Średni promień kół określono eksperymentalnie. Robot TRC Labmate został ustawiony prostopadle do ściany laboratorium i wykonał przejazd po linii prostej na odległość 3 m w kierunku ściany. Niezależnego od odometrii pomiaru przebytej odległości d true dokonano za pomocą skanera laserowego Sick LMS 200 zamontowanego w przedniej części robota. Wykonano skan przed startem i po zatrzymaniu się robota, a następnie wyznaczono różnicę zmierzonych odległości do ściany. Procedurę powtórzono 30 razy. Eksperyment ten oraz wszystkie następne wykonano na jednolitym, równym podłożu. Wyliczono wartość średnią pomiarów z odometrii d odo oraz ich odchylenie standardowe σ dodo, a także wartość średnią pomiarów ze skanera laserowego d true, którą przyjęto za estymatę rzeczywistej przebytej odległości (skaner LMS 200 charakteryzuje się bardzo małymi błędami pomiaru odległości, które uznano za pomijalne w porównaniu z błędami odometrii, por. p. 2.4.3). Odczytano stan enkoderów obu kół robota i wyznaczono wartości przyrostów kątów α l i α r. Następnie przekształcając wzory (2.5) i (2.6), uzyskano zależność pozwalającą wyznaczyć skorygowaną wartość średniego promienia kół estymatę promienia nominalnego: ˆr n = 2 d true α r + α l. (2.16) W wyniku eksperymentu otrzymano wartości: dodo = 2991,6 mm, σ dodo = 7,8 mm, d true = 2938,6 mm oraz wyznaczono skorygowaną wartość promienia kół ˆr n = 73,67 mm. W następnym eksperymencie wykonano test UMBmark dla robota TRC Labmate. Test wykonano w pomieszczeniu laboratoryjnym, w związku z czym konieczne było zmniejszenie długości boku kwadratu do l umb = 3 m. Podobnie jak w poprzednim eksperymencie, do określenia miejsca startu i zatrzymania robota użyto danych ze skanera laserowego Sick LMS 200, który dokonywał pomiarów odległości do dwóch prostopadłych względem siebie ścian laboratorium. Wykonano po 10 przejazdów w obu kierunkach, notując odchylenia x ecw, y ecw dla kierunku zgodnego z ruchem wskazówek oraz x eccw, y eccw dla kierunku przeciwnego. 21

A [mm] 300 200 B [mm] 300 200 y e 100 0-100 y e 100 0-200 -300-100 -200-400 -100 0 100 200 300 400 [mm] x e -300-50 0 50 100 150 200 250 300 350 400 [mm] x e Rysunek 2.2. Wyniki eksperymentów UMBmark dla robota TRC Labmate (opis w tekście) Dla uproszczenia pomiarów początek układu współrzędnych przyjęto w punkcie startu. Podczas odtwarzania ścieżki robot poruszał się z prędkością postępową 0,2 m s. Polecenia ruchu (translacji i rotacji) wykonywane były w trybie point-to-point, wykorzystującym trapezowe profile prędkości sterowników osi robota TRC Labmate [312], co wraz z niewielką prędkością zminimalizowało wpływ ewentualnych błędów przypadkowych na wyniki eksperymentu. Wyniki te przedstawiono na rys. 2.2A, gdzie okręgami zaznaczono odchylenie współrzędnych punktów końcowych ścieżki dla kierunku zgodnego z ruchem wskazówek zegara, a kwadratami dla kierunku przeciwnego. Zaznaczono też środki ciężkości obu grup punktów. Na podstawie eksperymentów otrzymano następujące wartości poszukiwanych współczynników: E d = 1,001, E b = 0,988 oraz wyznaczono skorygowaną wartość bazy kół robota ˆb = 336,05 mm, a także skorygowane promienie koła lewego ˆr l = 73,633 mm i koła prawego ˆr r = 73,706 mm. Skorygowane wartości promieni kół i bazy używane są następnie w niezależnej od wbudowanego sterownika robota implementacji odometrii, opartej na równaniach (2.5) i (2.7). W pracy [51] na podstawie testu UMBmark zdefiniowano także syntetyczny wskaźnik systematycznego błędu odometrii: ( e sysmax = max ( xecw )2 + (ȳ ecw )2, ) ( x eccw )2 + (ȳ eccw )2. (2.17) Pozwala on porównać dokładność odometrii różnych robotów lub ocenić, w jakim stopniu w procesie kalibracji został wyeliminowany systematyczny błąd odometrii danego robota. Na podstawie testu UMBmark robota TRC Labmate przed kalibracją uzyskano wartość e sysmax = 400,67 mm. Wykorzystując skorygowane wartości parametrów geometrycznych zrealizowano odometrię niezależną od istniejącego sterownika robota. Z jej wykorzystaniem powtórzono test UMBmark i otrzymano wyniki przedstawione na rys. 2.2B oraz wartość e sysmax = 114,4 mm. 2.2.3. Model niepewności odometrii Jak wykazała analiza przedstawiona w poprzednim podrozdziale, na niepewność pomiarów położenia i orientacji robota dokonywanych za pomocą odometrii składa się wiele czynni- 22

< ków. W praktyce dominują błędy systematyczne, które powinny być wyeliminowane lub istotnie ograniczone przez odpowiednią kalibrację. Uznając duże (katastrofalne) błędy przypadkowe, wynikające z nieprzewidywalnych interakcji z otoczeniem, za niepewność co do przedmiotu pomiaru, można założyć, że przyczyną niepewności położenia i orientacji robota mobilnego są głównie poślizgi kół oraz ich niepunktowy kontakt z podłożem. Poślizg koła powoduje niepewność jego elementarnego przemieszczenia liniowego ( d l lub d r ). Niepunktowy kontakt z podłożem może być wyrażony jako niepewność długości bazy b. A < r B b d k-1 k Rysunek 2.3. Graficzna interpretacja modelu niepewności odometrii (opis w tekście) Ponieważ przyrosty kąta obrotu koła lewego i prawego mierzone są przez niezależne enkodery, założono, że dla niewielkiego przyrostu przebytej drogi wartości d l i d r są nieskorelowane. Niezależne są także pomiary enkoderów w kolejnych (dyskretnych) chwilach. Biorąc pod uwagę powyższe właściwości systemu pomiarowego oraz zakładając skuteczną kompensację błędów systematycznych, za model matematyczny niepewności elementarnego przemieszczenia liniowego koła przyjęto rozkład normalny. Przemieszczenie jest interpretowane jako suma prawdziwej (nieznanej) wartości d i addytywnego błędu δd, będącego zmienną losową o zerowej wartości oczekiwanej i wariancji σ 2 d : ˆd = d + δd,δd = N(0,σ d ), (2.18) gdzie ˆd jest estymatą przemieszczenia, a σ d odchyleniem standardowym. Niepewność długości bazy b wynika z niepunktowego kontaktu kół (które są wulkanizowane warstwą kauczuku) z podłożem. Rzeczywiste punkty kontaktu obu kół z podłożem przemieszczają się w pewnym obszarze styku w sposób zależny od wielu czynników, np. chwilowych przyśpieszeń pojazdu, miejscowych właściwości podłoża. Biorąc to pod uwagę, za model matematyczny niepewności długości bazy przyjęto rozkład normalny. Baza kół jest sumą prawdziwej (nieznanej) wartości b i addytywnego błędu δb, będącego zmienną losową o zerowej wartości oczekiwanej i wariancji σ 2 b : gdzie ˆb jest estymatą bazy, a σ b odchyleniem standardowym. ˆb = b + δb,δb = N(0,σb ), (2.19) 23

Zadaniem odometrii robota mobilnego jest wyznaczanie położenia i orientacji pojazdu. Dla robota mobilnego naturalnym układem współrzędnych, w którym określane są położenie i orientacja, jest układ kartezjański, a parametry identyfikowanego modelu opisane są wektorem x R = [x R y R θ R ] T, przy czym x R,y R określają położenie, a θ R orientację robota względem zewnętrznego układu współrzędnych. Wektor x R będzie dalej nazywany pozycją robota mobilnego. Zakładając, że niepewność pozycji robota określonej za pomocą odometrii zależy od błędów przypadkowych, których rozkłady dane są zależnościami (2.18) i (2.19), można wyznaczyć niepewność wektora x R w postaci macierzy kowariancji C R : σx 2 σ yx σ θx C R = σ xy σy 2 σ θy, (2.20) σ xθ σ yθ σθ 2 dokonując propagacji niepewności pomiarów [289]. Zliczeniowy charakter odometrii powoduje, że w modelu niepewności pozycji robota należy uwzględnić także model procesu sposób, w jaki zmienia się niepewność wektora x R, gdy robot przechodzi ze stanu poprzedniego (k 1) do stanu bieżącego (k). Graficzną prezentację macierzy kowariancji stanowi elipsoida niepewności [289]. Elipsoida ta wyznacza linię stałego prawdopodobieństwa dla wielowymiarowego rozkładu normalnego. W przypadku dwóch zmiennych (współrzędnych położenia robota: x R,y R ) elipsoida staje się elipsą na płaszczyźnie. Znając wektor x R i macierz C R, można wykreślić elipsę, wewnątrz której z określonym prawdopodobieństwem znajduje się początek układu współrzędnych robota. By przedstawić błąd pozycjonowania robota w postaci elipsy, należy zredukować macierz C R do podmacierzy o wymiarach 2 2 zawierającej elementy związane ze zmiennymi x R i y R. Jeżeli macierz kowariancji jest diagonalna, to kontur prawdopodobieństwa ma postać elipsy ze środkiem w punkcie wyznaczonym przez wektor wartości średnich [x R y R ] T i osiami skierowanymi zgodnie z osiami kartezjańskiego układu współrzędnych. Jeżeli kowariancje są niezerowe, to kąt pomiędzy wielką osią elipsy a dodatnią półosią x układu współrzędnych można wyznaczyć na podstawie zależności: ϕ = 1 2 arctg 2σ xy σx 2 σy 2. (2.21) Po wprowadzeniu oznaczenia τ = (σx) 2 2 + (σy) 2 2 2σxσ 2 y 2 + 4σxy 2 długość połowy wielkiej i małej osi elipsy można wyrazić odpowiednio za pomocą wzorów: a maj = k 2 (σ2 x + σ 2 y + τ), a min = gdzie k jest stałą związaną z założoną wartością prawdopodobieństwa: k 2 (σ2 x + σ 2 y τ), (2.22) k = 2ln (1 P(x,y)). (2.23) W literaturze opisano wiele podejść do modelowania niepewności odometrii. Chatila i Laumond [67] przedstawiali niepewność odometrii za pomocą wartości skalarnej (liczby). Teoretyczna analiza niepewności odometrii z wykorzystaniem reprezentacji probabilistycznej jest przedstawiona w pracy [319]. Model ten pozwala na analizę niepewności pozycji podczas ruchu robota po dowolnym łuku, nakłada jednak ograniczenia na maksymalną 24

zmianę orientacji (kąt θ) w pojedynczym kroku. Reprezentację probabilistyczną w postaci macierzy kowariancji stosowano w większości późniejszych prac dotyczących odometrii [32, 68, 81, 159, 198, 207], jednak metoda jej wyznaczania nie zawsze jest oparta na głębszej analizie sposobu funkcjonowania odometrii robota mobilnego. Przykładem może być model odometrii przedstawiony w pracy [81], w którym założono, że niepewność pozycji wyznaczonej za pomocą odometrii jest wynikiem niepewności jednostkowego przyrostu drogi dla środka robota d i jednostkowej zmiany orientacji θ, które uznano za statystycznie niezależne. Takie podejście do modelowania niepewności odometrii jest stosunkowo popularne w literaturze przedmiotu, wykorzystane zostało np. w monografii [72] do robota Labmate, jednak opiera się ono na nieuzasadnionym założeniu (w przypadku robota o różnicowym układzie jezdnym) o niezależności błędów d i θ jak nietrudno zauważyć na podstawie równań (2.6) i (2.7), wielkości te są ze sobą skorelowane. W pracy [68] zwrócono uwagę na niespójność takiego modelu wynikowe macierze kowariancji otrzymywane dla tej samej ścieżki przy różnych długościach elementarnych przedziałów d i θ (co odpowiada wyznaczaniu niepewności odometrii z różną częstotliwością) są różne. Na rysunku 2.4A przedstawiono przykładową ścieżkę robota (test UMBmark) z naniesionymi elipsami niepewności położenia x R, y R wyznaczonymi według modelu z pracy [81] dla P = 95%. Elipsy zaznaczone cienką linią zostały wyznaczone dla d = 500 mm, a zaznaczone linią grubą dla d = 3000 mm. Widoczna jest istotna rozbieżność między obydwoma oszacowaniami niepewności położenia przy tej samej ścieżce i parametrach modelu. A B odleg³oœæ [mm] odleg³oœæ [mm] odleg³oœæ [mm] odleg³oœæ [mm] Rysunek 2.4. Niepewność położenia w różnych modelach odometrii (ścieżka testu UMBmark) Procedurę wyznaczania niepewności odometrii robota o różnicowym układzie jezdnym, zapewniającą spójność oszacowania niepewności pozycji wyliczonej dla różnych długości kroku d, przedstawili Chong i Kleeman [68]. Za wyjściową niepewność przyjęli oni niepewność drogi przebytej przez koło robota w danym kroku. Poprzez propagację wariancji na podstawie tej niepewności jest wyznaczana macierz kowariancji postaci (2.20). Model ten opisuje wyłącznie niepewność wynikającą z błędów przypadkowych podobnie jak w niniejszej pracy założono, że błędy systematyczne są usunięte przez odpowiednią kalibrację. Brane są pod uwagę jedynie błędy wynikające z rozbieżności między rzeczywistą długością drogi koła a drogą wyliczoną na podstawie wskazań enkoderów (por. rys. 2.3A). Nie są natomiast uwzględniane błędy wynikające z niepunktowego kontaktu kół z podłożem oraz z istnienia poślizgów poprzecznych do płaszczyzny obrotu koła (por. rys. 2.3B). Rozszerzenie mode- 25

lu z artykułu [68] o te błędy przedstawiono w pracy [161]. Model niepewności odometrii uwzględniający oba rodzaje błędów (dane zależnościami (2.18) i (2.19)), oparty na podejściu Chonga i Kleemana, opisano poniżej. Na podstawie równań kinematyki robota w układzie differential drive (2.1), (2.3) można zapisać model procesu dla odometrii tego rodzaju robota: ˆx R(k) = f(ˆx R(k 1),u (k) ), (2.24) ) f x (ˆx R(k 1),u (k) ) ˆx R(k 1) + dr+d l 2 cos (ˆθR(k 1) + dr d l 2b ) ˆx R(k) = f y (ˆx R(k 1),u (k) ) = ŷ R(k 1) + dr+d l 2 sin (ˆθR(k 1) + dr d l 2b, f θ (ˆx R(k 1),u (k) ) ˆθ R(k 1) + dr d l b gdzie u (k) jest wejściem w chwili k, złożonym z wyznaczonych za pomocą odometrii odległości przebytych przez koła od chwili k 1 oraz z długości bazy b: u = [d r d l b] T. Ponieważ poszczególne źródła błędów są statystycznie niezależne, niepewność wektora u dana jest diagonalną macierzą kowariancji: C u(k) = σd 2 r 0 0 0 σd 2 l 0 = 0 0 σb 2 k r d r(k) 0 0 0 k l d l(k) 0 0 0 k b b 2 d r(k) d l(k) b, (2.25) gdzie k r, k l i k b są współczynnikami proporcjonalności określającymi stosunek wariancji do przebytej odległości. Współczynniki k r i k l wyrażone są w milimetrach, a k b w radianach. Macierz kowariancji postaci (2.20) w kroku k dana jest równaniem: C R(k) = J fr C R(k 1) J T f R + J fu C u(k) J T f U, (2.26) gdzie J fr jest jakobianem równań (2.24) względem wektora x R (stanu robota): f x f x f x ) 1 0 dr+d l x y θ f J fr = y f y f y 2 sin (ˆθR + dr d l 2b ) x y θ = 0 1 dr+d l f θ f θ f 2 cos (ˆθR + dr d l 2b, (2.27) θ x y θ 0 0 1 a J fu jakobianem (2.24) względem wektora u: J fu = gdzie poszczególne elementy mają postać: f x = 1 ( d r 2 cos ˆθ R + d r d l 2b f x d l = 1 2 cos ( ˆθ R + d r d l 2b f x b = d2 r d 2 l 4b 2 f x d r f y d r f θ d r f x d l f y d l f θ d l f x b f y b f θ b ) d r + d l 2b ) + d r + d l 2b ( sin 26, (2.28) ( sin ( sin ˆθ R + d r d l 2b ˆθ R + d ) r d l, (2.29) 2b ), (2.30) ˆθ R + d r d l 2b ), (2.31)

f y = 1 ( d r 2 sin f y d l = 1 2 sin ( f θ d r = 1 b, ˆθ R + d ) r d l + d r + d l 2b 2b ˆθ R + d r d l 2b f y b = d2 r d 2 l 4b 2 ) d r + d l 2b ( cos f θ d l = 1 b, ( cos ( cos ˆθ R + d r d l 2b ˆθ R + d ) r d l, (2.32) 2b ), (2.33) ˆθ R + d r d l 2b ), (2.34) f θ b = d r d l b 2. (2.35) W pracy [68] wartości współczynników proporcjonalności k r i k l określane są metodą prób i błędów, co stanowi pewien niedostatek podanej tam procedury. W badaniach opisanych w niniejszej rozprawie zastosowano dwustopniową procedurę ustalania wartości współczynników modelu odometrii: Na pierwszym etapie określono współczynniki na podstawie analizy niedokładności elementarnych ruchów: ruchu po prostej dla k r i k l traktowanych łącznie, obrotu w miejscu dla k b. Na drugim etapie, wykorzystując dane zebrane do testu UMBmark, uściślono współczynniki, tak by wyznaczona z modelu elipsa niepewności położenia jak najdokładniej odpowiadała elipsie powstałej na podstawie danych eksperymentalnych. W przeprowadzonym wcześniej eksperymencie stwierdzono, że odchylenie standardowe odległości na dystansie 3 m wynosi 9,5 mm. Daje to początkową wartość k r = k l = 0,00316 mm. Współczynnik k b wstępnie określono na podstawie wyników eksperymentu, w którym robot wykonywał obrót w miejscu w zakresie kąta pełnego. Wykonano 30 obrotów: 15 w kierunku zgodnym ze wskazówkami zegara, pozostałe w kierunku przeciwnym. Wyznaczono odchylenie standardowe σ θodo = 2. Daje to wartość k b = σ π θ odo 180 2π = 9,7 10 5. Wszystkie eksperymenty wykonano z prędkością postępową kół równą 0,2 m s. Po wprowadzeniu powyższych danych do modelu opisanego równaniami (2.24), (2.25) i (2.26) przeprowadzono symulację przejazdu robota po ścieżce z testu UMBmark, wyznaczając co 500 mm elipsę niepewności położenia przy założonym prawdopodobieństwie 95%. Porównano uzyskaną w punkcie końcowym elipsę z rozrzutem wyników eksperymentu (na rys. 2.4B zaznaczonych małymi kwadratami) oraz z wyznaczoną na podstawie uzyskanej macierzy kowariancji elipsą niepewności położenia. Ponieważ przyjęty model odometrii uwzględnia tylko błędy przypadkowe, wyniki eksperymentu przed porównaniem skorygowano o znany błąd systematyczny. Następnie korygowano współczynniki modelu tak, by osiągnąć najlepszą zgodność elipsy z modelu i z danych eksperymentalnych. Na rysunku 2.4B przedstawiono wyniki dla najlepszego z testowanych zestawów parametrów: k r = 0,003, k l = 0,0025, k b = 0,2 10 5. 2.2.4. Podsumowanie Wiedza o przyczynach błędów odometrii oraz sposobie ich propagacji poprzez równania kinematyki pojazdu do niepewności położenia i orientacji jest potrzebna, by zredukować nie- 27

które błędy (część systematyczna), a także zbudować model niepewności pozycji robota estymowanej z odczytów enkoderów. Model niepewności pozycji w postaci macierzy kowariancji (2.20) jest z kolei konieczny do optymalnego łączenia estymaty pozycji otrzymanej z odometrii z danymi otrzymywanymi z sensorów zewnętrznych. Jest to szczególnie istotne, gdy do łączenia danych wykorzystuje się filtr Kalmana [145], którego skuteczność zależy od prawidłowego oszacowania niepewności informacji [256]. Analizując odometrię robota o różnicowym układzie jezdnym, wyróżniono trzy rodzaje błędów. Każdy z nich jest traktowany inaczej: Błędy wynikające z niespełnienia głównych założeń leżących u podstaw samolokalizacji zliczeniowej (ruch w płaskiej przestrzeni roboczej, brak oddziaływań sił zewnętrznych) powodują niepewność co do przedmiotu pomiaru. Ich wykrycie i korekcja możliwe są jedynie dzięki odwołaniu się do innych sensorów robota. Błędy systematyczne korygowane są dzięki wykorzystaniu testu UMBmark. Błędy przypadkowe zostały zidentyfikowane i uwzględnione w modelu niepewności wzorowanym na podejściu Chonga i Kleemana [68]. Zaproponowano dwustopniową procedurę określania parametrów tego modelu, w której wykorzystano dane zebrane w teście UMBmark. Przeprowadzone eksperymenty i wyznaczone współczynniki korekcyjne nie pozwoliły na całkowitą eliminację błędów systematycznych, co jest widoczne na rys. 2.2B. Prawdopodobną przyczyną tego stanu rzeczy jest istnienie innych źródeł błędów systematycznych niż niedokładna znajomość bazy i średnicy kół. Dokładne oględziny wykorzystywanej platformy Labmate ujawniły niewielkie odchylenie osi prawego koła od właściwego kierunku (zwichrowanie), co może wprowadzać dodatkowy błąd systematyczny oraz sprzyja przemieszczaniu się punktu styku koła z podłożem w czasie jazdy. Można tu zastosować ogólniejszą procedurę kalibracyjną, która nie odwołuje się do właściwości ścieżki powodujących ujawnienie się poszczególnych błędów. Procedurę taką, opartą na estymacji metodą najmniejszych kwadratów, zaproponowano w artykule [15], jednak przedstawione w publikacji [16] wyniki eksperymentów oraz wyniki porównania z metodą UMBmark nie wykazują znacznego wzrostu dokładności kalibracji, natomiast istotnie wzrosła komplikacja eksperymentu. Ocena przydatności tej metody oraz innych proponowanych w literaturze sposobów kalibracji odometrii [159] wymagać będzie dalszych badań eksperymentalnych. Przedstawiony model niepewności odometrii uwzględnia źródła błędów przypadkowych. Zapewnia on prawidłową propagację niepewności pomiaru między kolejnymi stanami, co pozwala na wyznaczanie macierzy kowariancji odometrii w dowolnych punktach dowolnych ścieżek o stałym zakrzywieniu szczególnymi przypadkami takiej ścieżki są ruch po prostej i obrót w miejscu. Wyznaczone eksperymentalnie wartości parametrów modelu odometrii (współczynników skalujących) zapewniają dobrą (w sensie jakościowym) predykcję błędów przypadkowych, co pokazano na rys. 2.4B. Należy jednak pamiętać, że współczynniki k r, k l i k b zależą od właściwości podłoża, po którym porusza się robot, oraz od osiąganych prędkości i przyśpieszeń. Nie są więc dla danego typu robota (a nawet konkretnego egzemplarza) wartościami uniwersalnymi i stałymi powinny być określane dla warunków danego eksperymentu, co z praktycznego punktu widzenia stanowi niedostatek proponowanego modelu. Rozwiązaniem jest zastosowanie procedury autokalibracji on-line. Teoretyczne podstawy takiej metody zostały zaproponowane w pracy [198], w której przedstawiono także wyniki 28

symulacji potwierdzających jej skuteczność w odniesienu do robota poruszającego się w prostym środowisku. Implementacja autokalibracji odometrii, która wymaga zintegrowania tej procedury z systemem samolokalizacji robota wykorzystującym sensory zewnętrzne, pozostaje tematem dalszych badań. Standardowa odometria oparta na odczytach z enkoderów cechuje się akceptowalną dokładnością w przypadku niewielkich prędkości robota i małych przemieszczeń. Część błędów odometrii można skorygować błędy systematyczne przez kalibrację, a przypadkowe, gdy dysponuje się danymi z redundantnych enkoderów [53] lub innych sensorów wewnętrznych [32]. Całkowita eliminacja źródeł błędów jest jednak niemożliwa. Także model niepewności odometrii nie ujmuje w pełni wszystkich pozostałych błędów, stąd jest on adekwatny jedynie do określonego zakresu warunków ruchu robota. Model taki jest jednak przydatny, gdyż zapewnia łatwo dostępną estymatę przemieszczenia robota, niezbędną do wykonania wielu bardziej złożonych zadań nawigacyjnych w szczególności samolokalizacji i tworzenia mapy otoczenia. 2.3. Dalmierze ultradźwiękowe 2.3.1. Budowa i fizyczne podstawy działania Działanie stosowanych w robotyce dalmierzy akustycznych opiera się na ultradźwiękach, czyli drganiach akustycznych (mechanicznych) o częstotliwości ponad 16 khz, niesłyszalnych dla ludzkiego ucha. W większości takich dalmierzy (zwanych sonarami, od angielskiego określenia sound navigation and ranging) wykorzystuje się impulsową metodę pomiaru, polegającą na wysłaniu przez sensor impulsu akustycznego i pomiarze czasu t e do jego powrotu. Na podstawie zmierzonego czasu i znanej prędkości propagacji dźwięku v son obliczana jest droga przebyta przez falę akustyczną, odpowiadająca podwojonej odległości r od obiektu, od którego powierzchni odbiła się ta fala: r = 1 2 v sont e. (2.36) Typowym sposobem określenia momentu powrotu echa jest detekcja progowa [169]. Za czas powrotu echa przyjmowany jest moment przekroczenia przez odbierany sygnał założonego poziomu (progu). Metoda ta pozwala na budowę dalmierza o prostej konstrukcji, jednak traci się wówczas informację o amplitudzie odbieranego sygnału. Dalmierze takie określa się w literaturze anglojęzycznej mianem sensorów TOF (ang. time of flight) [104]. Prędkość propagacji dźwięku v son zależy od środowiska, w którym się on rozchodzi. W powietrzu prędkość ta zależy głównie od temperatury, a w mniejszym stopniu od wilgotności i ciśnienia. Zależność prędkości dźwięku od temperatury powietrza opisana jest następującym empirycznym wzorem [158]: TC v son = v 0 + 1, (2.37) 273 gdzie T C jest temperaturą powietrza wyrażoną w stopniach Celsjusza, a v 0 = 331,4 m s prędkością rozchodzenia się dźwięku w powietrzu o temperaturze 0 C. Przyjmuje się, że prędkość dźwięku w typowej temperaturze pokojowej 20 C v son = 343,3 m s [35]. Istnieje możliwość 29

listki boczne oœ akustyczna son listek g³ówny przetwornik r r min Rysunek 2.5. Uproszczona geometria wiązki dalmierza ultradźwiękowego użycia układu kompensacji zmian temperatury [104, 157], jednak proste dalmierze ultradźwiękowe, stosowane jako standardowe wyposażenie wielu typów robotów mobilnych, nie mają takiego układu. Charakterystyka kierunkowa źródła fal ultradźwiękowych składa się z listka głównego oraz listków bocznych (rys. 2.5). Położenie i kształt listków bocznych są uzależnione od wytłumienia źródła oraz jego sprzężenia akustycznego z ośrodkiem, w którym wytwarzana jest fala dźwiękowa [227]. Obszar oddziaływania fali ultradźwiękowej można podzielić na pole bliskie (Fresnela), którego przekrój poprzeczny ma w przybliżeniu kształt źródła fali (ma średnicę przetwornika) i pole dalekie (Fraunhofera), w którym wiązka jest rozbieżna. Ze względu na interferencje nakładających się maksimów lokalnych fali pomiary mogą być dokonywane jedynie w polu dalekim, rozpoczynającym się w odległości r min od przetwornika pomiarowego: r min = ρ2, (2.38) 4λ son gdzie λ son jest długością fali, a ρ średnicą przetwornika (rys. 2.5). Energia emitowana maleje wraz z oddalaniem się od osi akustycznej. Jeżeli przetwornik będzie pobudzony pojedynczą częstotliwością, to kształt przekroju poprzecznego emitowanej wiązki będzie opisany funkcją Bessela pierwszego rodzaju i pierwszego rzędu [43]. Szerokość kątowa wiązki zmienia się wraz z wykorzystywaną częstotliwością dźwięku, a za kąt θ son, odpowiadający połowie szerokości wiązki, przyjmowane jest pierwsze zero funkcji Bessela. Zależność tę opisuje poniższy wzór [172]: ( 1,22λson θ son = arcsin ρ ). (2.39) Jeżeli emitowany sygnał jest złożeniem impulsów o wielu częstotliwościach, to wynikowe fale akustyczne nakładają się, a kształt przekroju wiązki, wyrażony amplitudą ciśnienia akustycznego w funkcji kąta, może być aproksymowany krzywą Gaussa: θ 2 2σ A(θ) = A maxθ e θ 2, (2.40) gdzie θ jest kątem odchylenia względem osi akustycznej, σ θ = θson 2, a A max θ jest amplitudą ciśnienia akustycznego w osi przetwornika (dla θ = 0 ) [33]. Ponieważ fala akustyczna generowana przez sonar rozchodzi się w obszarze o kształcie stożkowym, powierzchnia jej czoła rośnie wraz z przebytą odległością, a ciśnienie akustyczne maleje: 30

A(r) = A max θ,0 ( r r min ) 2, (2.41) gdzie A maxθ,0 jest początkową amplitudą ciśnienia akustycznego w odległości r 0 od przetwornika i w jego osi akustycznej, a r jest przebytą odległością. Podczas rozchodzenia się fali akustycznej w powietrzu jej osłabienie następuje także za sprawą mechanizmów absorpcji, zależnych od lepkości i przewodności cieplnej ośrodka [227]. Zależność tę wyraża następujący wzór: A(r) = A maxθ,0 e µair(r r0), (2.42) gdzie µ air jest współczynnikiem absorpcji energii fali w powietrzu wyrażonym w decybelach przez metr [104]. Zasadniczy składnik współczynnika absorpcji µ air jest proporcjonalny do kwadratu częstotliwości dźwięku [158]. Gdy fala akustyczna rozchodząca się w powietrzu napotyka powierzchnię, na granicy ośrodków występuje zazwyczaj jednocześnie odbicie zwierciadlane i rozproszone, jednak stosunek, w jakim oba te odbicia zachodzą, zależy od długości fali padającej i właściwości powierzchni odbijającej [4]. Odbicie zwierciadlane zachodzi, gdy wiązka ultradźwięków napotyka na swej drodze dużą (w stosunku do szerokości wiązki) i gładką powierzchnię. W takim przypadku kąt odbicia wiązki jest równy kątowi jej padania. Powierzchnia taka zachowuje się wobec wiązki ultradźwiękowej podobnie jak zwierciadło w przypadku światła. Echo od powierzchni zwierciadlanej może powrócić do sensora, gdy kąt pomiędzy osią wiązki a normalną do obserwowanej powierzchni jest mniejszy niż θ son. Odbicie rozproszone (dyfuzyjne) następuje, gdy wiązka ultradźwięków napotyka powierzchnię o nierównościach wielkości porównywalnej z λ son lub gdy kształty obiektu tworzą ostrą krawędź. Powierzchnia rozpraszająca jest dla sonaru widoczna w szerszym zakresie kątów niż powierzchnia zwierciadlana o takiej samej wielkości i kształcie [317]. W dalszej części rozdziału będą rozważane jedynie właściwości układów sensorycznych dokonujących pomiarów za pomocą pojedynczych sonarów z detekcją progową. Takie układy, często w postaci wieńca przetworników otaczającego korpus robota, są podstawowymi sensorami odległości wielu badawczych i dydaktycznych robotów mobilnych. Najbardziej rozpowszechnione dalmierze ultradźwiękowe stosowane w robotach mobilnych oparte są na elektrostatycznych przetwornikach pomiarowych Polaroid [241]. Są to sensory mierzące odległość metodą impulsową, charakteryzujące się zwartą budową, małymi wymiarami (rys. 2.6A) i niskim kosztem pojedynczego przetwornika. Ten sam przetwornik wykorzystywany jest przemiennie jako nadajnik i odbiornik ultradźwięków. Sam przetwornik pomiarowy przypomina budową kondensator. Składa się z wykonanej z aluminium okładziny stałej, o średnicy części aktywnej 38 mm, oraz ruchomej (sprężystej) kołowej membrany wykonanej z nieprzewodzącego tworzywa sztucznego pokrytego z jednej strony metalową folią. Umieszczenie między tymi okładzinami ładunku elektrostatycznego powoduje przyciągnięcie metalowej folii do okładziny stałej. Ruch membrany generuje falę akustyczną, mającą postać krótkiej paczki impulsów. Ta sama membrana, która w trybie nadawania emituje ukierunkowaną falę ultradźwiękową, w trybie odbioru pełni funkcję detektora fali odbitej. Spośród elektrostatycznych przetworników pomiarowych Polaroid w robotach mobilnych najczęściej stosowany jest Series 600 Instrument Grade Transducer. Dostarczany wraz z przetwornikiem pomiarowym układ elektroniczny zapewnia formowanie paczki impulsów oraz detekcję echa. W odbiorniku zastosowano wzmacniacz, którego 31

wzmocnienie wzrasta w funkcji czasu mierzonego od momentu wysłania impulsu, by kompensować osłabienie sygnału odbitego od dalszych obiektów. Przetworniki serii 600 mogą pracować z dowolną częstotliwością z zakresu od 40 do 100 khz, jednak moduł pomiarowy domyślnie generuje sygnał o częstotliwości f = 49, 4 khz, co oznacza falę o długości λ son 7 mm. Obszar propagacji fali sensora Polaroid w polu dalekim jest w przybliżeniu ściętym stożkiem o średnicy przy wierzchołku równej średnicy przetwornika pomiarowego i kształcie określonym przez kąt θ = 14,5 (wzór 2.39, rys. 2.5). Rzeczywista charakterystyka kierunkowa dalmierza, przedstawiona w materiałach producenta [241] i zamieszczana w innych publikacjach (np. [66, 104, 169]), ma po dwa listki boczne o maksimach odpowiednio dla kątów około 20 i 30 względem osi akustycznej. A B Rysunek 2.6. Dalmierz Polaroid (A) i sonary robota Labmate (B) Rysunek 2.7. Sonary robota Pioneer 2DX Maksymalny zasięg sonarów ograniczony jest wymaganą minimalną mocą powracającego sygnału. Nominalny zasięg użyteczny podawany przez producenta wynosi 10,5 m. Zasięg minimalny determinowany jest czasem koniecznym do zmiany trybu pracy przetwornika oraz wielkością pola bliskiego charakterystyki kierunkowej. Na podstawie danych nominalnych przetwornika i wyliczonego r min przyjęto zasięg minimalny równy 0,05 m. Czas pomiaru pojedynczym sonarem zależy od mierzonej odległości. W praktyce częstotliwość powtarzania pomiaru z użyciem jednego sensora jest nie większa niż 15 Hz. W wykorzystywanym w Laboratorium Robotów Mobilnych IAiII systemie TRC Proximity [313] sonary Polaroid są rozmieszczone równomiernie (rozstawione co 15 ) na palecie w kształcie okręgu (rys. 2.6B). Badano także przydatność innych ustawień sensorów, okazały się one jednak mniej efektywne [194]. Robot Pioneer 2DX ma natomiast integralny moduł ośmiu sonarów Polaroid umieszczonych w jego przedniej części (rys. 2.7). 2.3.2. Charakterystyka dalmierza Polaroid Niepewność pomiaru odległości w kontrolowanym środowisku By zidentyfikować źródła niepewności pomiarów wpływające na ich rozrzut oraz określić użyteczny zakres pomiarowy sensora Polaroid, przeprowadzono kilka eksperymentów w kontrolowanym środowisku. Polegały one na badaniu dokładności pomiaru pojedynczym czujnikiem odległości od wybranych obiektów, ustawionych w znanej odległości od przetwornika 32

pomiarowego. Obserwowane powierzchnie tych obiektów tworzyły znany kąt względem osi akustycznej sensora. W eksperymentach wykorzystano zestaw sonarów Polaroid zamontowanych na robocie Labmate. odleg³oœæ zmierzona [cm] 900 800 700 600 500 400 300 200 100 bezwzglêdny b³¹d pomiaru [cm] 20 0-20 -40-60 -80-100 -120 0 0-140 100 200 300 400 500 600 700 800 900 1000 0 0 100 200 300 400 500 600 700 800 900 1000 0 100 200 300 400 500 600 700 800 900 1000 odleg³oœæ rzeczywista [cm] odleg³oœæ rzeczywista [cm] odleg³oœæ rzeczywista [cm] odchylenie standardowe [cm] 150 100 50 Rysunek 2.8. Wyniki badania dalmierza Polaroid w kontrolowanym środowisku W pierwszym eksperymencie starano się zapewnić dobre warunki pomiaru, eliminując znane źródła zakłóceń zewnętrznych. Pomiary wykonano w dużym, zamkniętym pomieszczeniu o wymiarach około 15 5 m i gładkich ścianach. W czasie pomiarów w pomieszczeniu panowała stała temperatura 20 C. Wybrany przetwornik z wieńca sonarów pozycjonowano prostopadle do krótszej ściany pomieszczenia, a następnie przemieszczano w zakresie odległości od 0,5 do 10 m od tej ściany, wykonując pomiary odległości co 0,5 m. W każdym położeniu wykonano 30 pomiarów. Celem tego eksperymentu było określenie użytecznego zakresu pomiaru odległości i dokładności pomiaru w funkcji odległości. liczba pomiarów odleg³oœci 400 350 300 300 A B C liczba pomiarów odleg³oœci 250 200 200 250 200 150 150 150 100 100 100 50 50 50 0 0 0 950 960 970 980 990 1000 1010 1020 1030 1040 1050 2960 2980 3000 3020 3040 3060 3080 4960 4980 5000 5020 5040 5060 5080 5100 5120 zmierzona odleg³oœæ [mm] zmierzona odleg³oœæ [mm] zmierzona odleg³oœæ [mm] liczba pomiarów odleg³oœci 300 250 Rysunek 2.9. Histogramy pomiarów odległości: A 1 m, B 3 m, C 5 m Na rysunku 2.8 przedstawiono bezwzględny błąd pomiaru oraz odchylenie standardowe uzyskane dla wiązki padającej prostopadle na płaską powierzchnię ściany. Widoczna jest duża dokładność pomiaru w takiej konfiguracji sensora. Aranżując eksperyment w kontrolowanym środowisku, za pomocą sonarów Polaroid można uzyskać precyzyjne pomiary odległości od wybranych obiektów. Także powtarzalność wartości mierzonej jest duża. Względny błąd pomiaru podany przez producenta sensorów Polaroid wynosi 2% wartości wielkości mierzonej [313]. Dla badanego (wybranego w sposób przypadkowy) egzemplarza wartość ta jest prawdziwa, gdy mierzona odległość wynosi do 5 m. Powyżej tego dystansu względny błąd pomiaru rośnie wraz ze wzrostem odległości mierzonej, np. dla 8 m wynosi już około 5%. W zakresie do 5 m powtarzalność pomiarów jest bardzo duża. Powyżej tej odległości rozrzut wartości pomiaru jest funkcją mierzonej odległości. W warunkach zbliżonych do poprzedniego doświadczenia dokonano też oceny rozrzutu wartości mierzonej odległości dla trzech wybranych dystansów: 1, 3 i 5 m. Celem tego 33

bezwzglêdny b³¹d pomiaru [cm] 0-5 -10-15 -20-25 -30-35 -40 0 2 4 6 8 10 12 14 o k¹t padania wi¹zki na powierzchniê [ ] odchylenie standardowe pomiaru [cm] 35 30 25 20 15 10 5 0 0 2 4 6 8 10 12 14 o k¹t padania wi¹zki na powierzchniê [ ] Rysunek 2.10. Wpływ kąta padania wiązki na powierzchnię na dokładność pomiaru eksperymentu było empiryczne określenie rozkładu gęstości prawdopodobieństwa wyników pomiarów odległości. Dla każdej z trzech wybranych odległości wykonano 1000 pomiarów, zachowując w przybliżeniu stałe warunki zewnętrzne. Na rysunku 2.9 przedstawiono uzyskane histogramy pomiarów odległości. Na histogramy nałożono krzywe Gaussa, wykreślone dla wartości oczekiwanych i odchyleń standardowych wyznaczonych z danych pomiarowych. Na podstawie wyników tego eksperymentu można stwierdzić, że rozkład normalny dobrze oddaje charakter rozrzutu wyników pomiarów odległości dokonywanych dalmierzem ultradźwiękowym. 2 12 bezwzgêdnyl b³¹d pomiaru [cm] 0-2 -4-6 -8-10 -12 Karton Styropian Wyk³adzina Wyt³oczka odchylenie standardowe [cm] 10 8 6 4 2 Karton Styropian Wyk³adzina Wyt³oczka -14 50 100 150 200 250 300 350 400 rzeczywista odleg³oœæ od przeszkody [cm] 0 50 100 150 200 250 300 350 400 rzeczywista odleg³oœæ od przeszkody [cm] Rysunek 2.11. Wpływ rodzaju powierzchni na dokładność pomiaru sonarem Poprawne wyniki pomiarów można uzyskać jedynie wówczas, gdy wiązka ultradźwiękowa pada na przeszkodę w przybliżeniu prostopadle. Wraz ze zwiększeniem kąta padania następuje gwałtowny wzrost błędów pomiaru odległości. Zjawisko to ilustruje rys. 2.10, na którym przedstawiono wyniki pomiarów odległości od gładkiej ściany laboratorium sonarem ustawionym w odległości 3 m. Sensor ustawiono na wprost ściany, prostopadle do jej powierzchni, a następnie stopniowo odchylano co 1 w zakresie do 15 z zachowaniem stałej odległości od ściany mierzonej w osi wiązki pomiarowej. Pomiary wykonano tylko dla kątów dodatnich, zakładając symetrię charakterystyki kierunkowej sonaru. W każdym położeniu wykonano 30 pomiarów odległości. Do pomiarów wykorzystano sonary robota Labmate. 34

Uznano jednak, że pozycjonowanie robota z wymaganą precyzją nie będzie w tym eksperymencie możliwe, więc pomiarów dokonano pojedynczym przetwornikiem pomiarowym umocowanym na statywie fotograficznym. Na rysunku 2.10 można zaobserwować, że błąd bezwzględny pomiaru zachowuje małą wartość dla kątów padania wiązki nie większych niż 5. Dla większych kątów błąd rośnie, a powyżej 12 pomiary stają się nieużyteczne ze względu na bardzo duży błąd, sięgający 30% mierzonej odległości. Wraz ze wzrostem kąta padania wiązki na obserwowaną powierzchnię szybko rośnie odchylenie standardowe. Zbadano także wpływ różnych ro- dzajów powierzchni na wynik pomiaru odległości dalmierzem ultradźwiękowym. Badanie to prowadzono w podobnych warunkach do warunków eksperymentu poprzedniego. Badany sensor ustawiono prostopadle do statywu, na którym montowano kolejno plansze o wymiarach około 40 60 cm wykonane z następujących materiałów: gładkiej tektury (karton), styropianu, wykładziny dywanowej i falistej tektury (wytłoczka). Dobierając materiały, starano się odzwierciedlić właściwości typowych powierzchni spotykanych w takich środowiskach, jak biura i pomieszczenia mieszkalne. Następnie badaną liczba przypadków wykrycia obiektu 0 o 100 80 60 40 20 0 0 5 o 20 40 10 o 60 80 100 15 o 120 400 350 300 250 200 150 100 odleg³oœæ [cm] Rysunek 2.12. Wyniki eksperymentu z obiektem cylindrycznym powierzchnię przemieszczano w stronę dalmierza, wykonując co 0,5 m serię 30 pomiarów. Zakres badanych odległości został ograniczony do 4 m ze względu na rozmiary pomieszczenia laboratorium. Uzyskane wyniki przedstawiono na rys. 2.11 w postaci wykresów bezwzględnego błędu pomiaru i odchylenia standardowego w funkcji rzeczywistej odległości od obserwowanej powierzchni. Widoczne jest zróżnicowanie wyników w zależności od właściwości materiału. Najmniejsze błędy bezwzględne i odchylenie standardowe odnotowano dla płyty styropianowej, największe dla pofałdowanej tektury. Miękka powierzchnia (wykładzina) nie spowodowała większych błędów pomiaru odległości. Dla wszystkich typów materiałów uzyskano zadowalające wyniki pomiarów (błąd względny nie większy niż 2% mierzonej odległości i duża powtarzalność wyników) jedynie wówczas, gdy dystans wynosił do około 3 m. Należy zwrócić uwagę, że wyniki tego eksperymentu mogą być obarczone większą niepewnością niż eksperymenty poprzednie, gdyż użyto stosunkowo małych powierzchni, a na wyniki pomiarów mogły wpływać odbicia od innych przedmiotów w laboratorium lub samego uchwytu i podstawy, do których umocowano obserwowane obiekty. Ostatni eksperyment miał na celu określenie rzeczywistej szerokości kątowej wiązki dalmierza Polaroid oraz stwierdzenie, jak kształtuje się prawdopodobieństwo wykrycia przeszkody w zależności od jej położenia kątowego względem osi akustycznej sonaru. W eksperymencie tym wybranym sonarem robota Pioneer 2DX mierzono odległość od tekturowej tuby ustawionego pionowo długiego cylindra o średnicy 15 cm. Obiekt ustawiano w odległości kątowej od 0 do 20 od osi dalmierza z krokiem równym 5, a odległość zmieniano od 40 do 400 cm z krokiem równym 10 cm. W celu prawidłowego ustalenia położeń obiektu 50 0 35

posłużono się naniesioną na podłodze laboratorium siatką kątową. Podobnie jak we wcześniejszym eksperymencie (rys. 2.10), pomiary wykonano tylko dla kątów dodatnich. W każdym punkcie powtórzono pomiar 100 razy, notując, czy obiekt został wykryty. Za wykrycie obiektu uznawano wynik pomiaru odległości różniący się od odległości nominalnej o wartość mniejszą niż średnica cylindra. Wyniki eksperymentu przedstawiono na rys. 2.12, na którym pokazano, ile razy obserwowany obiekt został wykryty w zależności od kąta względem osi sensora i odległości od niego. Użyty w eksperymencie tekturowy cylinder był wykrywany do odległości około 3 m, jeżeli znajdował się w pobliżu osi akustycznej sonaru. Graniczny kąt wykrywania obiektu wynosił około 10, w pomiarach przy kącie 15 cylinder nie był już wykrywany nawet z minimalnej odległości. Wykrywanie obiektu przy kącie 10 z mniejszej odległości niż odległość odnotowana dla kątów mniejszych można tłumaczyć oddziaływaniem listków bocznych charakterystyki kierunkowej sensora, których emitowana energia jest mniejsza. Nie odnotowano pomiarów w zakresach kątów odpowiadających trzeciemu listkowi charakterystyki być może niewielka powierzchnia obserwowanego cylindra spowodowała, że echa pochodzące od tego maksimum były zbyt słabe i nie zostały wykryte. Wyniki przedstawionych powyżej eksperymentów pozwalają sformułować następujące wnioski: Jeżeli obserwowany obiekt znajduje się w osi akustycznej dalmierza, a jego powierzchnia ustawiona jest do tej osi prostopadle, to błędy pomiaru odległości są stosunkowo niewielkie, a ich główną przyczyną jest zmiana amplitudy powracającego sygnału. Amplituda sygnału w odbiorniku, zależna od energii powracającego echa, ma wpływ na dokładność pomiaru odległości [104, 169]. Na rysunku 2.8 można zaobserwować wzrost błędu pomiaru w funkcji odległości. Błąd ten ma charakter systematyczny, a jego zależność od zmierzonej odległości może być aproksymowana ciągłą funkcją nieliniową. Biorąc pod uwagę zasadę pomiaru odległości w sonarze z detekcją progową, można założyć, że efekt ten wynika z opisanego wzorami (2.41) i (2.42) osłabienia echa wraz z przebytą drogą. W dalmierzu osłabienie to jest kompensowane przez wspomnianą już zmianę wzmocnienia odbieranego sygnału, jednak zmiana ta następuje skokowo (ang. stepped gain) [104], co prowadzi do błędnego określenia czasu powrotu echa słabszych sygnałów. Inne źródła błędów to: niestałość amplitudy generowanego sygnału akustycznego, szumy w układzie elektronicznym odbiornika (szum termiczny) oraz zewnętrzne szumy w zakresie ultradźwięków. Zwiększenie kąta padania wiązki na powierzchnię obserwowaną powoduje istotny wzrost zarówno błędu pomiaru odległości, jak i odchylenia standardowego. W przypadku powierzchni, dla których dominuje odbicie dyfuzyjne, wzrost błędu spowodowany jest większym osłabieniem wiązki odbitej pod większym kątem. W przypadku powierzchni, dla których dominuje odbicie zwierciadlane, mechanizm powstawania błędu jest bardziej skomplikowany. Jeżeli obserwowana jest płaska powierzchnia (ściana), to decydujące znaczenie ma szerokość kątowa wiązki. Pomiar odległości następuje wraz z powrotem do sensora pierwszego echa wystarczająco silnego, by przekroczyć próg detekcji. Jednak gdy kąt α pomiędzy osią akustyczną sensora a mormalną do obserwowanej powierzchni jest większy, echo to może pochodzić ze względu na szerokość wiązki od odbicia zwierciadlanego z kierunku prostopadłego do obserwowanej powierzchni, czyli odpowiadać najmniejszej odległości sensora od ściany, a nie odległości mierzonej wzdłuż osi akustycznej. W literaturze podawana jest infor- 36

macja, że w sonarach Polaroid serii 600 zjawisko to występuje dla kątów powyżej α max = 12 [4, 66], co potwierdzają wyniki przedstawione na rys. 2.10, gdzie dla tej wartości kąta następuje gwałtowny wzrost błędu bezwzględnego pomiaru odległości. Podczas obserwacji pod dużym kątem obiektów dających odbicia typu zwierciadlanego możliwa jest też rejestracja echa pochodzącego od listków bocznych charakterystyki kierunkowej. Odbicia fali akustycznej od powierzchni różniących się właściwościami fizycznymi, takimi jak chropowatość i stopień pochłaniania (tłumienia) drgań mechanicznych, różnią się stopniem osłabienia echa, co w rezultacie daje różne wielkości błędów pomiaru odległości. Różna faktura badanych materiałów powoduje też zróżnicowanie stopnia odbicia zwierciadlanego i dyfuzyjnego. Przyczyny błędów w pomiarach odległości Przeprowadzona wyżej statystyczna analiza pomiarów odległości wykonanych w specjalnie przygotowanej konfiguracji otoczenia uwidoczniła, że w zakresie do około 5 m wyniki pomiarów sonarem Polaroid są wystarczająco dokładne na potrzeby systemu sterowania robota mobilnego. Jednak eksperymenty z powierzchniami pokrytymi różnymi materiałami oraz zmianą orientacji względem obserwowanej płaszczyzny wskazały na podstawowe przyczyny małej wiarygodności pomiarów pojedynczym sonarem. Są to zjawiska związane z charakterem odbić fal ultradźwiękowych od przeszkód. Większość obiektów spotykanych w typowym środowisku laboratoryjnym, biurowym lub przemysłowym (ściany, pionowe płaszczyzny mebli, maszyn itp.) nie ma nierówności o wymiarach porównywalnych ze stosowaną długością fali, przez co w znikomym stopniu rozprasza wiązkę ultradźwięków, powodując odbicia zwierciadlane. Odbicia tego rodzaju powodują, że wysłany impuls powraca do przetwornika jedynie wtedy, gdy został odbity od obserwowanego obiektu w kierunku prostopadłym do płaszczyzny sensora. W przeciwnym razie echo nie powraca do sensora (rys. 2.13A). Może także zajść sytuacja, w której echo powróci po odbiciu się od powierzchni kilku kolejnych obiektów w otoczeniu robota (rys. 2.13B). Zmierzony czas propagacji fali ultradźwiękowej będzie w tym przypadku odpowiadał długości drogi (s, p 1, p 2, p 3, s), a nie podwojonej długości odcinka (s, p 1 ) w osi akustycznej sensora. Błąd ten wprowadza niepewność co do przedmiotu pomiaru, która nie może być zredukowana metodami analizy statystycznej wielokrotne powtórzenie pomiaru w tej samej konfiguracji sensora i otoczenia daje powtarzalne wyniki, a odchylenie standardowe nie odbiega od wartości typowych dla prawidłowych pomiarów. A robot wi¹zka padaj¹ca wi¹zka odbita B robot s wi¹zka padaj¹ca p 3 p 1 wi¹zka odbita p 2 C wi¹zka padaj¹ca i wi¹zka odbita robot Rysunek 2.13. Przykłady pomiarów obarczonych niepewnością co do ich przedmiotu 37

Potencjalnym źródłem błędnych pomiarów są także zjawiska związane z odebraniem przez nasłuchujący sensor echa impulsu innego niż właśnie nadany. Może to być spowodowane powrotem do sensora opóźnionego echa poprzedniego impulsu (co jest możliwe wskutek wielokrotnych odbić przy pomiarach powtarzanych w krótkich odstępach) lub odebraniem przez dany sensor echa impulsu wysłanego wcześniej przez inny przetwornik (rys. 2.13C) znajdujący się na tym samym robocie. Zjawiska tego rodzaju, znane pod nazwą przesłuchów (ang. crosstalks), są skutkiem użycia wielu przetworników pracujących z tą samą częstotliwością. Przesłuchy mogą być w znacznym stopniu wyeliminowane przez zastosowanie odpowiednio dobranej sekwencji uruchamiania sensorów robota [50]. W systemach wielorobotowych poważny problem mogą stanowić przesłuchy pomiędzy sensorami ultradźwiękowymi różnych robotów, jeżeli operują one na tej samej częstotliwości. Model niepewności pomiarów Z powyższych rozważań i przedstawionych wyników eksperymentów wynika, że dominującym rodzajem niepewności w pomiarach dalmierzami ultradźwiękowymi jest niepewność co do przedmiotu pomiaru. W związku z tym w proponowanym modelu matematycznym dalmierza ultradźwiękowego zakłada się, że rozpatrywany pomiar odległości interpretowany jest w sposób właściwy, tzn. niepewność co do przedmiotu pomiaru jest eliminowana dzięki zastosowaniu odpowiednich algorytmów działających na poziomie systemu sterowania robota mobilnego, np. dzięki porównywaniu i agregacji obserwacji w różnych pozycjach robota. Dodatkowym ograniczeniem są użyteczne zakresy pomiaru odległości i kąta padania wiązki na obserwowaną powierzchnię. Biorąc pod uwagę wyniki eksperymentów przedstawione na rys. 2.8 i 2.10, przyjęto maksymalną odległość mierzoną sonarem r max = 5 m oraz dopuszczalny kąt padania wiązki na powierzchnię obserwowaną α max = 12. Pomiary odległości wykraczające poza r max będą ignorowane jako mało wiarygodne. Zadaniem dalmierza jako czujnika robota mobilnego jest wykrywanie obiektów w otoczeniu i identyfikacja ich położenia względem robota. Dla dalmierza naturalnym układem współrzędnych, w którym dokonywane są pomiary, jest układ biegunowy, a parametrami identyfikowanego modelu obserwowanego obiektu są odległość r i kąt ϕ w układzie współrzędnych związanym z robotem (por. rys. 2.16). Odległość jest mierzona sensorem, natomiast nominalna wartość kąta ϕ odpowiadająca położeniu osi akustycznej wynika z ustawienia sonaru względem korpusu robota, np. z jego pozycji w wieńcu czujników. Nie jest tu rozważany przypadek sonaru ze skanowaniem mechanicznym choć takie konstrukcje są znane z literatury [33, 80, 157], to nie są one powszechnie stosowane. Odległość r jest wyznaczana z zależności (2.36) na podstawie pomiaru czasu powrotu echa. Błędy systematyczne wynikają przede wszystkim z zależności parametrów rozchodzenia się fali akustycznej od układu warunków fizycznych podczas pomiaru. Błąd systematyczny wprowadzany jest przez różnicę rzeczywistej prędkości dźwięku w stosunku do prędkości v son przyjmowanej we wzorze (2.36). Jeżeli istnieje możliwość pomiaru temperatury powietrza wokół robota, to błąd spowodowany różnicą temperatury otoczenia i temperatury nominalnej można zniwelować, stosując zależność (2.37). Pozostałe składniki błędu systematycznego związane są głównie z niedokładnym pomiarem czasu powrotu echa. Błąd ten można wyeliminować, jeżeli dostępny jest pomiar amplitudy powracającego echa [34]. W przypadku dalmierza z detekcją progową można jedynie uwzględnić poprawkę określoną na podstawie otrzymanej doświadczalnie zależności między bezwzględnymi błędami i wartościami zmierzonej odległości. Zależności takie otrzymano 38

bezwzglêdny b³¹d pomiaru [cm] 2 0-2 -4-6 -8-10 -12-14 -16 50 100 150 200 250 300 350 400 450 500 odleg³oœæ zmierzona [cm] bezwzglêdny b³¹d pomiaru [cm] 2 0-2 -4-6 -8-10 -12-14 Karton Styropian Œciana -16 0 50 100 150 200 250 300 350 400 450 500 odleg³oœæ zmierzona [cm] Rysunek 2.14. Wyniki kalibracji błędu systematycznego w pomiarze odległości sonarem w prezentowanych wyżej eksperymentach i przedstawiono na rys. 2.8 oraz 2.11. Za podstawę wyznaczenia poprawki przyjęto dane z rys. 2.8, gdyż uznano je za bardziej reprezentatywne dla rzeczywistych sytuacji, w których może się znaleźć robot mobilny (pomiar odległości od gładkiej ściany). Poprawkę wyznaczono jedynie w zakresie pomiaru odległości uznanym uprzednio za użyteczny. Za funkcję aproksymującą zależność między błędem bezwzględnym a wartością zmierzoną r = f( r m ) przyjęto równanie kwadratowe postaci: r = a 0 + a 1 r m + a 2 r 2 m, (2.43) gdzie a i,i = 0...2, są poszukiwanymi współczynnikami, a r m wartością średnią zmierzonej odległości. Z wykorzystaniem pakietu Matlab przeprowadzono dopasowanie w sensie minimum sumy kwadratów odchyłek między danymi eksperymentalnymi a modelem. Otrzymano następujące wartości współczynników: a 2 = 3,917 10 5, a 1 = 1,379 10 2, a 0 = 1,227. Dla wielomianu (2.43) suma kwadratów odchyłek między danymi a modelem s 2 = 0,291. W wyniku aproksymacji wielomianem trzeciego stopnia otrzymano jedynie minimalnie lepsze dopasowanie (s 3 = 0,281), uznano więc wielomian kwadratowy za wystarczający. Na rysunku 2.14A przedstawiono uzyskane z eksperymentu wartości r (punkty) i dopasowaną krzywą postaci (2.43) (linia ciągła). Na rysunku 2.14B pokazano natomiast błędy bezwzględne pomiaru odległości z uwzględnieniem poprawki dla trzech zestawów danych: pomiarów do ściany (dane będące podstawą aproksymacji, linią ciągłą pokazano błędy bez poprawki), pomiarów do płyty styropianowej i pomiarów do arkusza gładkiego kartonu. Wprowadzenie poprawki powoduje redukcję bezwzględnych błędów pomiaru odległości w przyjętym zakresie użytecznym dalmierza, należy jednak pamiętać, że najczęstsze błędy pomiaru powstają wskutek interakcji wiązki ultradźwiękowej i obiektów o określonej geometrii, w związku z czym stosowalność wyznaczonej poprawki ograniczona jest do sytuacji, gdy sonar obserwuje pojedynczy obiekt o znacznej powierzchni. W praktyce przesłanki do zastosowania poprawki istnieją jedynie wówczas, gdy a priori dany jest model otoczenia, w którym dominują płaskie ściany, lub gdy dostępna jest informacja z innych sensorów. Zakładając, że odczyt odległości mieści się w użytecznym zakresie pomiarowym, a błędy systematyczne zostały wyeliminowane, przyjęto, że niepewność odległości r zależy od losowego składnika niepewności pomiaru czasu powrotu echa t e. Niepewność ta uwarunkowana jest głównie zmianą amplitudy sygnału w odbiorniku, wynikającą z osłabienia echa wraz 39

z przebytą odległością, oraz kształtem obiektu, od którego odbiła się fala. Biorąc pod uwagę wyniki doświadczenia przedstawione na rys. 2.9 oraz informacje podawane w literaturze [104], za model matematyczny rozrzutu wyników pomiarów odległości dokonywanych dalmierzem ultradźwiękowym przyjęto rozkład normalny. Pomiar odległości jest interpretowany jako suma prawdziwej (nieznanej) wartości pomiaru r i addytywnego błędu δr, będącego zmienną losową o zerowej wartości oczekiwanej i wariancji σ 2 r: ˆr = r + δr,δr = N(0,σ r ), (2.44) odchylenie standardowe [cm] 7 6 5 4 3 2 1 0 50 100 150 200 250 300 350 400 450 500 gdzie ˆr jest estymatą pomiaru, a σ r odchyleniem standardowym zależnym od wielkości mierzonej. Nieznana wartość r musi być zastąpiona dostępną oceną zmierzonej odległości, np. wartością średnią z serii pomiarów r m. Odchylenie standardowe także może być określone na podstawie wystarczająco licznej próby. W pracy [195] na podstawie analizy wyników doświadczenia podobnego do pierwszego eksperymentu przedstawionego w niniejszym rozdziale zaproponowano jako estymator wartości rzeczywistej mierzonej odległości wartość dominującą (dominantę). Zastosowanie tego estymatora także wymaga wystarczająco licznej próby, przy czym w pracy [195] nie określono jednoznacznie jej wartości. 8 W praktycznych zastosowaniach roboodleg³oœæ zmierzona [cm] Rysunek 2.15. Aproksymacja wielomianem (2.45) zależności odchylenia standardowego σ r od odległości tów mobilnych powtarzanie pomiarów jest z reguły niemożliwe z uwagi na ruch robota i stosunkowo długi czas pomiaru odległości sonarem. Jeżeli nie ma możliwości wielokrotnego powtórzenia pomiaru, to należy uznać, że ˆr jest bieżącym odczytem, będącym w danej chwili jedyną dostępną estymatą mierzonej odległości. W takiej sytuacji σ r jest wyznaczana jako niepewność typu B. Zależność odchylenia standardowego od zmierzonej odległości w przypadku obserwacji obiektu o dużej, jednolitej powierzchni została określona doświadczalnie i przedstawiona na rys. 2.8. Zależność tę aproksymowano za pomocą wielomianu postaci: σ r = a 0 + a 1 r m +...a n r n m, (2.45) gdzie a n są poszukiwanymi współczynnikami. Poszukując dopasowania w sensie minimum sumy kwadratów odchyłek między danymi eksperymentalnymi a modelem i wykorzystując do obliczeń pakiet Matlab, dla wielomianu rzędu n = 2 otrzymano wartość sumy kwadratów odchyłek s 2 = 0,219, dla n = 3 s 3 = 0,201, a dla aproksymacji funkcją liniową (n = 1) s 1 = 0,701, co pozwoliło uznać przybliżenie funkcją kwadratową za wystarczające. Dla n = 2 otrzymano następujące wartości współczynników: a 2 = 4,317 10 5, a 1 = 7,176 10 3, a 0 = 0,455. Na rysunku 2.15 przedstawiono odchylenie standardowe wyznaczone z pomiarów i dopasowaną krzywą postaci (2.45) (linia ciągła). Istotną wielkością wpływającą na niepewność pomiaru dalmierzem ultradźwiękowym jest także szerokość kątowa wiązki. Z uwagi na dużą szerokość wiązki niemożliwa jest precyzyj- 40

Y oœ akustyczna son < r 0 X Rysunek 2.16. Graficzna interpretacja modelu niepewności pomiaru sonarem na identyfikacja położenia obiektów za pomocą pojedynczego, nieruchomego sonaru. Odczytana wartość pomiaru odległości r pozwala jedynie stwierdzić, że obserwowany obiekt znajduje się na łuku zamykającym trójkąt równoramienny o wysokości r i kącie rozwarcia 2θ son. Wyniki eksperymentu z obserwacją cylindrycznego obiektu (rys. 2.12) wskazują, że dokonanie wiarygodnego pomiaru odległości jest bardziej prawdopodobne, gdy przeszkoda znajduje się w pobliżu osi akustycznej sonaru. Biorąc pod uwagę zależność amplitudy wysyłanego sygnału od kąta względem osi wiązki, daną wzorem (2.40), za model rozrzutu wyników identyfikacji położenia kątowego obiektu wewnątrz wiązki przyjęto rozkład normalny. Kąt ϕ w układzie współrzędnych związanym z robotem dla obserwowanego obiektu znajdującego się wewnątrz wiązki sonaru może być przedstawiony jako suma prawdziwej (nieznanej) wartości ϕ i addytywnego błędu δϕ, będącego zmienną losową o rozkładzie normalnym: ˆϕ = ϕ 0 + δϕ, δϕ = N(0,σ θ ) dla ϕ ϕ 0 θ son, (2.46) gdzie ϕ 0 jest nominalną wartością kąta ϕ odpowiadającą orientacji osi akustycznej sonaru względem układu robota, a σ θ = θson 2 odchyleniem standardowym określonym jako niepewność typu B, zgodnie z matematycznym modelem rozkładu amplitudy echa [33]. Na rysunku 2.16 przedstawiono graficzną interpretację modelu niepewności pomiaru dla wybranej wiązki sonaru umieszczonego w wieńcu sensorów. 2.3.3. Podsumowanie Wyeliminowanie większości z opisanych wyżej źródeł niepewności pomiaru, szczególnie dotyczących przedmiotu pomiaru, jest trudne, gdyż niepewność ta zależy od właściwości i położenia obiektów w otoczeniu robota, a te czynniki w przypadku robota autonomicznego są na ogół nieznane. Bezpośrednia (geometryczna), a zarazem wiarygodna interpretacja wyników pomiarów sonarami jest możliwa jedynie wówczas, gdy towarzyszy jej analiza sposobu propagacji ultradźwięków w powietrzu oraz ich odbić od przeszkód. Ponieważ postać echa zależy głównie 41

od kształtu obiektu, od którego nastąpiło odbicie fali, kluczowe znaczenie dla metod geometrycznej interpretacji pomiarów ma klasyfikacja obserwowanych przeszkód. Dla poszczególnych rodzajów obiektów budowane są odrębne modele procesu pomiarowego, pozwalające prawidłowo interpretować wynik pomiaru, a tym samym zidentyfikować położenie wykrytych przeszkód względem robota. W pracy [43] przedstawiono metodę rozróżniania pomiarów odległości od płaskich powierzchni (ścian) oraz krawędzi tworzonych przez dwie powierzchnie. Analizując amplitudę powracającego echa, rozróżnić można także pomiary od narożników [33]. Podejście geometryczne, pozwalające rozróżniać, klasyfikować i śledzić niektóre obserwowane sonarem obiekty, znalazło zastosowanie w samolokalizacji robotów mobilnych [181]. Jego wadą jest jednak konieczność przyjęcia odpowiedniej strategii akwizycji wyników pomiarów, gdyż pomiary odległości muszą być dokonywane bardzo gęsto, z małą separacją kątową. Alternatywą jest użycie bardziej zaawansowanej obróbki sygnału niż prosta detekcja progowa. Pierwszy z tych sposobów wydłuża czas procesu pomiarowego, drugi zazwyczaj nie może być wykorzystany do prostych zestawów sonarów komercyjnych robotów mobilnych. Jak dotąd, najlepsze wyniki przyjęcia podejścia geometrycznego otrzymano, badając układy złożone z większej liczby przetworników pomiarowych, z których najczęściej jeden działa jako nadajnik i odbiornik, a pozostałe jako odbiorniki [169]. Analizując literaturę, można stwierdzić, że podejście geometryczne do analizy danych z dalmierzy ultradźwiękowych jest stosowane obecnie głównie w odniesieniu do złożonych układów sonarów [34, 35, 160, 161, 170]. W przypadku robotów mobilnych wykonujących pomiary odległości pojedynczymi sonarami dominują różne warianty map rastrowych. Takie podejście przyjęto także w dalszej części niniejszej pracy. Pomiary odległości sensorami ultradźwiękowymi są wykorzystywane jedynie w sposób pośredni, po przetworzeniu na mapę rastrową jako uzupełnienie danych z dalmierzy laserowych. Przeprowadzona w tym rozdziale analiza niepewności pomiaru posłuży do budowy modeli matematycznych pozwalających na aktualizację mapy rastrowej. 2.4. Skanery laserowe 2.4.1. Budowa i fizyczne podstawy działania Dalmierze i skanery laserowe to sensory aktywne, wypromieniowujące energię świetlną w kierunku obserwowanych obiektów i odbierające energię odbitą od nich. Dalmierze laserowe są powszechnie używane w geodezji i budownictwie, mają też wiele zastosowań militarnych. Na potrzeby robotyki zaczęto je stosować w połowie lat siedemdziesiątych dwudziestego wieku [225]. Obecnie urządzenia te są wykorzystywane w systemach nawigacji robotów mobilnych. Tego rodzaju sensory mają wiele zalet w porównaniu z pasywnymi systemami wizyjnymi. Najistotniejsze zalety to niezależność od naturalnego oświetlenia sceny i zdolność do bezpośredniego pomiaru odległości, bez skomplikowanego i czasochłonnego przetwarzania obrazu. Przewaga dalmierzy optycznych nad ultradźwiękowymi wynika z właściwości medium pomiarowego bardzo mała długość fal świetlnych powoduje, że większość obiektów rozprasza wiązkę pomiarową równomiernie, nie powodując odbić zwierciadlanych. Wiązka optyczna ma też niewielką szerokość i rozbieżność (kąt rozwarcia), co pozwala uzyskać nieporównywalnie większą niż w sonarach rozdzielczość kątową. W niniejszej pracy skupiono się na dalmierzach skanujących (dalej zwanych w skrócie skanerami), czyli urządzeniach odchylających wiązkę pomiarową o pewien kąt w płaszczyźnie poziomej (skaner 2D) lub w obu płaszczyznach (skaner 3D). Skanery laserowe, zwłaszcza 42

umożliwiające skanowanie przestrzenne (3D), nazywane są także lidarami (ang. light detection and ranging). Pomiaru odległości w skanerach laserowych dokonuje się na podstawie czasu przebiegu impulsu światła odbitego od obserwowanego obiektu, przesunięcia fazy modulowanej fali świetlnej odbitej od przeszkody lub metodą triangulacyjną. Inne zasady pomiaru wykorzystywane w dalmierzach laserowych mają w robotyce marginalne znaczenie [126]. Najprostszą budową charakteryzują się dalmierze i skanery laserowe oparte na metodzie triangulacji. Takie dalmierze aktywne składają się ze źródła światła i odbiornika (np. matrycy lub linijki CCD), których wzajemne położenie jest znane (odległość pomiędzy źródłem i detektorem nazywana jest bazą), a ich osie optyczne się przecinają [104, 130]. Pod wpływem promienia światła z dalmierza na obserwowanej powierzchni powstaje jasny punkt, którego obraz rejestruje odbiornik. Położenie punktu o największej intensywności na otrzymanym obrazie jest jednoznacznie zależne od odległości od obserwowanego obiektu, którą można wyznaczyć, znając geometrię dalmierza [26]. Zaletą dalmierzy triangulacyjnych jest możliwość zastosowania jako źródeł światła tanich, niemodulowanych laserów [325]. Z punktu widzenia wykorzystania w robotach mobilnych wadą dalmierzy triangulacyjnych jest ograniczony zasięg (poniżej 5 m). Wadą metody triangulacyjnej jest także niemożność wykonania pomiaru, gdy obiekt oświetlony wiązką pomiarową nie jest widziany przez odbiornik. Sytuacja ta występuje tym rzadziej, im bliżej położone są nadajnik i odbiornik, zmniejszenie bazy powoduje jednak spadek rozdzielczości. Inne metody pomiaru odległości za pomocą aktywnych sensorów optycznych opierają się na bezpośrednim lub pośrednim określeniu czasu upływającego między wysłaniem sygnału świetlnego i odebraniem tego sygnału odbitego od przeszkody. W przeciwieństwie do sensorów triangulacyjnych w dalmierzach opartych na pomiarze czasu odbiornik jest umieszczany blisko nadajnika najlepiej gdy wiązka wysyłana i odbita od przeszkody usytuowane są współosiowo [6]. Metoda określania odległości przez pomiar przesunięcia fazowego ciągłej fali świetlnej o modulowanej sinusoidalnie amplitudzie jest stosowana w celu ominięcia problemów technicznych związanych z bezpośrednim pomiarem czasu przebiegu impulsu światła. Metoda ta określana jest w literaturze anglojęzycznej skrótem AMCW (ang. Amplitude Modulated Continuous Wave) [126]. Mierzona odległość jest proporcjonalna do różnicy faz między falą wysyłaną przez sensor a falą odbitą od przeszkody. Jako nadajniki stosowane są diody laserowe, np. GaAs, GaAsAl [13], lub diody elektroluminescencyjne (LED) o dużej mocy promieniowania [4, 206]. Jeżeli amplituda nadawanego sygnału modulowana jest pojedynczą częstotliwością f AM, to pomiar przesunięcia fazowego φ d między sygnałem nadawanym (odniesienia) a odbitym pozwala wyznaczyć odległość: φ d c r = φ d = 1 4πf AM 2 λ AM 2π, (2.47) gdzie λ AM jest długością fali modulacji, a c oznacza prędkość światła. Ponieważ przesunięcie fazy mierzone jest w przedziale kąta 2π, wyznaczona odległość jest jednoznaczna jedynie w zakresie r u = 1 2 λ AM. Zwiększenie zakresu jednoznaczności w określeniu przesunięcia fazowego można uzyskać, stosując podwójną modulację amplitudy z dwoma różnymi częstotliwościami [133]. Błąd pomiaru odległości zależy głównie od dokładności określenia przesunięcia fazowego φ d. Precyzyjny pomiar fazy sygnału odbitego stanowi zasadniczą trudność techniczną w budowie dalmierzy laserowych AMCW. Jedno z możliwych rozwiązań tego problemu zaproponowano w pracy [238], gdzie pomiar fazy sygnału powracającego 43

zastąpiono strojeniem częstotliwości modulacji, tak by różnica fazy wynosiła zawsze 90. Z zasady pomiaru (2.47) wynika, że jego niepewność, wyrażona wariancją mierzonej odległości, jest wprost proporcjonalna do niepewności pomiaru różnicy fazy: σ 2 r = ( λam 4π ) 2 σ 2 φ. (2.48) W typowym dalmierzu AMCW [4, 5, 206] faza sygnału o amplitudzie V φ jest mierzona przez wykrycie w odbiorniku jego przejścia przez zero. Sygnał ten powstaje w fotodetektorze, będącym źródłem zmiennego w czasie prądu proporcjonalnego do powracającego sygnału optycznego. Jako detektorów najchętniej używa się obecnie fotodiod PIN i diod lawinowych APD. Diody APD, oparte na efekcie powielania, umożliwiają uzyskanie lepszego stosunku sygnału odbieranego do szumu niż ich tańsza alternatywa diody PIN oraz charakteryzują się większą czułością. Powstający sygnał V φ jest zakłócony przede wszystkim ze względu na: szum fotonowy [225], szumy elektroniczne detektora głównie szum prądu ciemnego, termiczny i śrutowy [326], szum powodowany oświetleniem zewnętrznym. Moc zakłóceń zależy od konkretnego typu detektora i warunków jego pracy. Porównanie stosunku sygnału użytecznego do szumu dla diod PIN i APD przedstawiono w artykule [326], natomiast w książce [4] dokładnej analizie poddano źródła zakłóceń w detektorze opartym na diodzie APD. Niezależnie jednak od konstrukcji układów elektronicznych danego dalmierza AMCW (której szczegóły w przypadku urządzeń komercyjnych zazwyczaj nie są znane), po założeniu stałych warunków zewnętrznych wszystkie źródła szumów elektronicznych można przedstawić jako zakłócenie V n o zerowej wartości średniej i stałej wariancji σ 2 n [4]. Odrębnym źródłem zakłóceń jest szum fotonowy, który staje się istotny, gdy intensywność odbieranego światła jest niewielka [225]. Zakładając, że może być on wyrażony jako zakłócenie V p o wariancji σ 2 p, sygnał elektryczny opuszczający detektor można opisać jako V φ = V r cos(ωt + φ) + V n + V p, (2.49) gdzie V r jest amplitudą proporcjonalną do mocy sygnału odbitego od przeszkody. Można wykazać [206], że stosunek odchylenia standardowego σ Σ, reprezentującego sumaryczne zakłócenia w torze pomiaru fazy dalmierza AMCW, do odchylenia standardowego mierzonej fazy σ φ jest wyrażony poniższym wzorem: σ Σ σ φ = V φ (ωt) Vφ =0, (2.50) którego prawa strona wyraża zmianę amplitudy odebranego sygnału w momencie jego przejścia przez zero. Biorąc pod uwagę równanie (2.49), wzór (2.50) można wyrazić jako σ Σ σ φ = V r. (2.51) Po uwzględnieniu poczynionych powyżej założeń sumaryczną wariancję zakłóceń wyraża wzór: σ 2 Σ = σ 2 n + σ 2 p. (2.52) 44

Na podstawie równań (2.51) i (2.52) nietrudno zauważyć, że wariancja mierzonej fazy jest proporcjonalna do wariancji zakłóceń w torze odbiornika, a odwrotnie proporcjonalna do kwadratu amplitudy powracającego sygnału: ( 1 σφ 2 = V r ) 2 (σ 2 n + σ 2 p). (2.53) Zakładając, że emisja fotonów jest procesem Poissona, można wykazać [225], że odchylenie standardowe szumu fotonowego jest proporcjonalne do pierwiastka kwadratowego amplitudy odebranego sygnału: σ p = k p Vr, (2.54) gdzie k p jest stałym współczynnikiem proporcjonalności. Wyrażenie opisujące wariancję mierzonej odległości można wyprowadzić, biorąc pod uwagę wzory (2.48), (2.53) i (2.54): σ 2 r = ( ) 2 ( ) 2 λam σ n 1 + 4π V r ( ) 2 ( ) λam k p 1 + σ 4π V e, 2 (2.55) r gdzie σ 2 e jest stałą wariancją pozwalającą uwzględnić w modelu dodatkowe źródła szumów elektronicznych wprowadzanych w torze pomiarowym po detekcji fazy [4]. Równanie (2.55) obrazuje charakter zależności między jakością pomiaru odległości a amplitudą powracającego do sensora sygnału odbitego od przeszkody. Jeżeli intensywność odbieranego światła nie jest bardzo mała, to szum fotonowy można zaniedbać [4, 225], a zależność opisującą wariancję mierzonej odległości można uprościć do postaci: σ 2 r ( ) 2 ( ) 2 λam σ n 1. (2.56) 4π V r Na podstawie powyższych rozważań oraz literatury (np. [3]) można stwierdzić, że informacja o natężeniu (amplitudzie) powracającego sygnału jest niezwykle istotna dla prawidłowej interpretacji pomiarów odległości dalmierzem AMCW. Stąd w wielu dalmierzach i skanerach, szczególnie opracowanych specjalnie do celów badawczych, stosowany jest dodatkowy kanał pomiaru natężenia światła odbitego. Powyższa analiza niepewności pomiaru pokazuje także, jak istotne jest uzyskanie jak największego stosunku sygnału do szumu w torze obiornika. W dalmierzach AMCW możliwość poprawy tego parametru przez zwiększenie mocy nadajnika ograniczona jest warunkami bezpieczeństwa pracy z laserami [223, 239]. Urządzenia te charakteryzują się ciągłą emisją laserową, co powoduje, że zachowanie klasy bezpieczeństwa I (bezpieczne dla oka) jest możliwe jedynie w przypadku zastosowania diody LED lub diody laserowej o niewielkiej mocy. Skanery AMCW buduje się przez wyposażenie dalmierza opartego na tej metodzie pomiaru w odpowiedni układ optomechaniczny z ruchomym zwierciadłem. W bardziej złożonych skanerach 3D stosowane są układy z wirującymi zwierciadłami wielokątowymi (poligonalnymi) [147, 237]. Dalmierze laserowe oparte na metodzie bezpośredniego pomiaru czasu przebiegu światła są urządzeniami klasy TOF, podobnie jak sonary. Mierzą one czas, który upłynął między wysłaniem impulsu świetlnego przez nadajnik a odebraniem impulsu odbitego od przeszkody. Krótkie impulsy światła otrzymuje się za pomocą modulatorów elektrooptycznych lub ze źródeł światła pracujących impulsowo [82]. Na podstawie pomiaru czasu t TOF od chwili emisji 45

impulsu do czasu powrotu impulsu odbitego od obserwowanego obiektu można wyznaczyć odległość r do tego obiektu: r = 1 2 ct TOF. (2.57) Ponieważ nadany impuls porusza się z prędkością światła, to w przypadku niewielkich odległości (kilku metrów) i rozdzielczości rzędu centymetrów konieczny jest pomiar czasu z dokładnością pikosekundową, co stanowi zasadniczą trudność techniczną w budowie dalmierzy laserowych TOF. Stosowane są analogowe układy pomiaru czasu oparte na ładowaniu pewnej pojemności stałym prądem w danym przedziale czasu lub układy cyfrowe zliczające impulsy referencyjnego oscylatora [144]. Mimo koncepcyjnej prostoty zasada bezpośredniego pomiaru czasu ustępowała popularnością metodzie AMCW w zastosowaniach robotycznych z powodu trudności i wysokich kosztów budowy układów [93]. Sytuacja ta uległa zmianie wraz z pojawieniem się komercyjnych skanerów TOF, produkowanych seryjnie, a zaprojektowanych głównie na potrzeby automatyki przemysłowej (bariery bezpieczeństwa) [263] oraz zastosowań w budownictwie i transporcie [248]. Obecnie skanery laserowe TOF są sensorami o najlepszych parametrach w rozważanej klasie aktywnych sensorów optycznych. Wykorzystanie impulsowej emisji laserowej o nanosekundowych czasach trwania impulsu pozwoliło zastosować w tych urządzeniach źródła światła spójnego o większej mocy z zachowaniem I klasy bezpieczeństwa. To z kolei poprawiło stosunek sygnału do szumu w odbiorniku i pozwoliło na dokładne pomiary odległości od powierzchni o różnych właściwościach optycznych (różnym stopniu pochłaniania światła) [168]. Skanery laserowe spełniające wymagania charakterystyczne dla poszczególnych zastosowań robotów mobilnych są nadal budowane jako prototypy badawcze [41]. 2.4.2. Charakterystyka skanera zbudowanego w KARiI Budowa skanera Do budowy tego eksperymentalnego skanera (rys. 2.17) wykorzystano komercyjną głowicę pomiaru odległości (dalmierz) firmy Pepperl & Fuchs GmbH, której działanie jest oparte na zasadzie pomiaru przesunięcia fazowego emitowanej i odbitej fali ciągłej o modulowanej amplitudzie. Źródłem fali optycznej jest dioda elektroluminescencyjna pracująca w zakresie promieniowania podczerwonego 880 nm [138]. Fala jest modulowana amplitudowo z częstotliwością 8 MHz. Gdy częstotliwość modulacji amplitudy f AM = 8 MHz, zakres jednoznaczności pomiarów wynosi 18,75 m. Jednak zasięg zastosowanego dalmierza, wynikający z mocy nadajnika i czułości odbiornika, jest znacznie mniejszy i wynosi (według danych producenta) od 30 do 5000 mm dla powierzchni o rozmiarach 200 mm 200 mm i o współczynniku odbicia światła odpowiednio od 18% do 90%. Nominalna rozdzielczość pomiaru wynosi 10 mm (wyniki odczytywane są w centymetrach). Maksymalna częstotliwość powtarzania pomiarów odległości nie przekracza 100 Hz [324]. Dalmierz Pepperl & Fuchs, produkowany jako element automatyki przemysłowej, nie ma możliwości pomiaru amplitudy powracającego sygnału, co jest jego istotną wadą w kontekście zastosowania jako precyzyjnego sensora robota autonomicznego. Skaner wyposażono w mechanizm odchylania wiązki, pozwalający wykonywać pomiary na scenie trójwymiarowej. Zakres skanowania wynosi 45 w pionie i 360 w poziomie. Z pola obserwacji wyłączony jest jedynie niewielki obszar zajmowany przez wspornik głowicy pomiarowej (rys. 2.18). Możliwość pracy w trybie 3D nie jest wykorzystywana w nawigacji 46

NADAJNIK ODBIORNIK r ODCHYLANE WIRUJ CE ZWIERCIAD O Rysunek 2.17. Skaner AMCW zbudowany w KARiI Rysunek 2.18. Schemat budowy skanera KARiI robota mobilnego ze względu na czas akwizycji danych, dla pełnego skanu przestrzennego wynoszący 120 s, oraz złą jakość pomiarów odległości powyżej 2 m od urządzenia. W trybie skanowania w jednej płaszczyźnie liczba pomiarów odległości (pełen skan 2D) wynosi 280 w zakresie 360. Czas wykonania pełnego skanu 2D wynosi 2,8 s. Połączenie komunikacyjne pomiędzy komputerem robota mobilnego a sterownikiem skanera stanowi łącze szeregowe. Dokładny opis budowy układu mechanicznego i sterującego skanera zamieszczono w pracy [138]. Niepewność pomiaru odległości w kontrolowanym środowisku W celu określenia charakterystyk dalmierza wykonano eksperymenty w kontrolowanym środowisku. Polegały one na badaniu dokładności pomiaru odległości od wybranych obiektów, ustawionych w znanej odległości od sensora, przy wyłączonym mechanizmie obrotu zwierciadła. Na drodze wiązki optycznej umieszczono płaską płytę, której odległość rzeczywistą r t od punktu przyjętego za początek układu współrzędnych skanera (punkt przecięcia płaszczyzny lustra odchylającego przez oś optyczną nadajnika) zmieniano w granicach od 50 do 400 cm. Wykonano 280 1 pomiarów co 50 cm. Badano wpływ współczynnika odbicia światła obiektu oraz kąta padania wiązki na obserwowaną płaszczyznę na wyniki pomiarów odległości. Pomiary przeprowadzono dla pięciu powierzchni arkuszy papieru o różnych współczynnikach odbicia światła (stopniach szarości). Każda z tych powierzchni była obserwowana pod czterema różnymi kątami φ i tworzonymi przez normalną do powierzchni i oś wiązki pomiarowej: 0, 20, 40 i 60. W celu dokładnego ustawienia kąta oś obrotu płaszczyzny odbijającej umieszczono tak, aby była prostopadła do osi wiązki optycznej. Eksperymenty prowadzono przy sztucznym oświetleniu i stałej temperaturze 20 C w pomieszczeniu laboratorium. Na podstawie otrzymanych pomiarów wyliczono dla wszystkich kombinacji rodzaju powierzchni i kąta padania wiązki wartość średnią r, błąd bezwzględny r = r r t i odchylenie standardowe σ r pomiaru. Otrzymane wartości przedstawiono na wykresach w funkcji odległości rzeczywistej r t. W ten sposób została wyznaczona charakterystyka przetwarza- 1 Taka liczba pomiarów jest przesyłana jednorazowo przez sterownik skanera. 47

A 400 D 400 wartoœæ zmierzona [cm] 350 300 250 200 150 100 wartoœæ zmierzona [cm] 350 300 250 200 150 100 B b³¹d bezwzglêdny [cm] 50 50 100 150 200 250 300 350 400 10 0-10 -20-30 -40 wartoœæ rzeczywista [cm] E b³¹d bezwzglêdny [cm] 50 50 100 150 200 250 300 350 400 20 0-20 -40-60 -80 wartoœæ rzeczywista [cm] C -50 50 100 150 200 250 300 350 400 6 5.5 5 wartoœæ rzeczywista [cm] F -100 50 100 150 200 250 300 350 400 4 3.5 wartoœæ rzeczywista [cm] odchylenie standardowe [cm] 4.5 4 3.5 3 2.5 2 odchylenie standardowe [cm] 3 2.5 2 1.5 1.5 1 50 100 150 200 250 300 350 400 wartoœæ rzeczywista [cm] 1 50 100 150 200 250 300 350 400 wartoœæ rzeczywista [cm] Bia³y Szary 20% Szary 40% Szary 60% Szary 80% 0 o 20 o 40 o 60 o Rysunek 2.19. Wyniki badania skanera KARiI w kontrolowanym środowisku (opis w tekście) nia toru pomiaru odległości skanera r = f(r t ), a także zależność błędu bezwzględnego i odchylenia standardowego od mierzonej odległości. Na rysunkach 2.19A, B i C przedstawiono wyniki pomiarów skanerem AMCW uzyskane dla powierzchni w różnym stopniu odbijających światło. Widoczne jest zróżnicowanie wyników w zależności od stopnia szarości powierzchni, jednak charakterystyki przetwarzania mają zbliżony kształt. Na rysunku 2.19B widać, że dla ciemniejszych powierzchni błędy bezwzględne rosną. Rozrzut wartości pomiaru rośnie wraz ze zmierzoną odległością, zależy też od stopnia pochłaniania światła przez daną powierzchnię. Z przedstawionej na rys. 2.19C 48

zależności σ r = f(r t ) wynika, że powyżej odległości około 2 m odchylenie standardowe szybko wzrasta tym szybciej, im ciemniejsza jest obserwowana powierzchnia. odleg³oœæ zmierzona [cm] 400 350 300 250 200 150 100 50 BIA Y CZARNY BR ZOWY ALUMINIUM 0 0 50 100 150 200 250 300 350 400 bezwzglêdny b³¹d pomiaru [cm] 50 0-50 -100-150 -200 BIA Y CZARNY BR ZOWY ALUMINIUM -250 0 50 100 150 200 250 300 350 400 odleg³oœæ rzeczywista [cm] odleg³oœæ rzeczywista [cm] odleg³oœæ rzeczywista [cm] odchylenie standardowe [cm] 6 5 4 3 2 1 BIA Y CZARNY BR ZOWY ALUMINIUM 0 0 50 100 150 200 250 300 350 400 Rysunek 2.20. Dokładność pomiarów skanerem KARiI dla różnych powierzchni Na rysunkach 2.19D, E i F przedstawiono wyniki uzyskane dla różnych kątów padania wiązki na białą powierzchnię. Funkcja przetwarzania skanera jest nieliniowa, co widać na rys. 2.19D. Nieliniowość jest tym wyraźniejsza, im większy jest kąt padania wiązki na przeszkodę. Dokładne pomiary odległości, niezależnie od kąta padania, można otrzymać w zakresie do około 1,5 m. Gdy odległości rzeczywiste są większe od 150 cm, wartości zmierzone są mniejsze od rzeczywistych tym mniejsze, im większy jest kąt φ i. Jedynie dla kąta φ i = 0 względny błąd pomiaru odległości mieści się w podanym przez producenta dalmierza zakresie 5% wartości mierzonej w całym badanym zakresie odległości. Pomiary uzyskane dla kątów φ i > 60 należy uznać za mało użyteczne z punktu widzenia systemu nawigacji robota autonomicznego, gdyż nawet w przypadku białej powierzchni ich błąd bezwzględny przekracza 10 cm już dla odległości poniżej 2 m. Rozrzut wartości pomiaru jest funkcją zmierzonej odległości oraz kąta padania wiązki. Z otrzymanej zależności σ r = f(r t ) (rys. 2.19F) wynika, że powyżej odległości 2 m odchylenie standardowe wzrasta tym bardziej, im większy jest kąt padania wiązki. Papier o różnych odcieniach szarości użyty w opisanym wyżej eksperymencie nie odzwierciedla wszystkich właściwości rzeczywistych powierzchni spotykanych w środowisku pracy robota [13]. Z tego powodu w dalszych badaniach wykorzystano różne materiały: biały styropian, karton w kolorach brązowym (gładki) i czarnym (matowy) oraz folię aluminiową. Na rysunku 2.20 przedstawiono charakterystyki przetwarzania oraz wykresy błędu bezwzględnego r i odchylenia standardowego σ r w zależności od odległości rzeczywistej r t. Błąd bezwzględny pomiaru nie przekracza 10 cm dla odległości do 2 m (co odpowiada błędowi względnemu 5% wielkości mierzonej) jedynie w przypadku użycia białego styropianu i folii aluminiowej, dla pozostałych materiałów błędy są znacznie większe. Dla większych odległości błąd pomiaru narasta w przypadku wszystkich materiałów. Dla matowej, czarnej tektury błąd bezwzględny przekracza 50 cm już w odległości r t = 2 m, co czyni pomiary odległości bezużytecznymi. Przebieg odchylenia standardowego σ r uzyskanego z serii pomiarów w zależności od odległości rzeczywistej potwierdza zauważoną w poprzednich eksperymentach silną zależność σ r od właściwości optycznych obserwowanej powierzchni. Oceniono rozrzut wartości mierzonej odległości dla trzech wybranych dystansów: 1 m (rys. 2.21A, D), 2 m (rys. 2.21B, E) i 3 m (rys. 2.21C, F). Ponieważ w poprzednich eksperymentach stwierdzono istotne różnice w pomiarach odległości od powierzchni jasnych i ciemnych, oceniono rozrzut dla białego papieru i czarnej, matowej tektury. Na rysunku 2.21 przedstawiono uzyskane histogramy pomiarów odległości, na które nałożono krzywe Gaussa, 49

liczba pomiarów odleg³oœci liczba pomiarów odleg³oœci 300 250 200 150 100 50 A B C 0 800 850 900 950 1000 1050 1100 1150 1200 300 250 200 150 100 50 zmierzona odleg³oœæ [mm] 300 250 200 150 100 50 0 1800 1850 1900 1950 2000 2050 2100 2150 2200 zmierzona odleg³oœæ [mm] D E F 0 800 850 900 950 1000 1050 1100 1150 1200 zmierzona odleg³oœæ [mm] liczba pomiarów odleg³oœci liczba pomiarów odleg³oœci 300 250 200 150 100 50 0 1200 1300 1400 1500 1600 1700 1800 1900 2000 zmierzona odleg³oœæ [mm] liczba pomiarów odleg³oœci liczba pomiarów odleg³oœci 300 250 200 150 100 50 0 2600 2650 2700 2750 2800 2850 2900 2950 3000 3050 3100 300 250 200 150 100 50 zmierzona odleg³oœæ [mm] 0 1200 1400 1600 1800 2000 2200 2400 2600 2800 3000 zmierzona odleg³oœæ [mm] Rysunek 2.21. Histogramy pomiarów odległości skaneram KARiI od powierzchni białej (A, B, C) i czarnej (D, E, F) wykreślone dla wartości oczekiwanych i odchyleń standardowych wyznaczonych z serii pomiarów. Na podstawie wyników tego eksperymentu można stwierdzić, że rozkład normalny dobrze oddaje charakter rozrzutu wyników pomiarów odległości dokonywanych dalmierzem laserowym typu AMCW. Nie stwierdzono istotnej zmiany charakteru rozkładu wyników w zależności od rodzaju i barwy powierzchni. Wyniki przedstawionych eksperymentów pozwalają sformułować następujące wnioski: Najistotniejszym składnikiem niepewności pomiarów odległości skanerem AMCW jest błąd systematyczny. Wartość tego błędu zależy nie tylko od mierzonej odległości, lecz także od kąta padania wiązki pomiarowej na obserwowaną powierzchnię oraz od właściwości optycznych samej powierzchni. Zależności te powodują, że kalibracja sensora AMCW, polegająca na wyznaczeniu zależności odległości rzeczywistej od odległości zmierzonej r m = f(r t ), nie jest wystarczająca. Sposób powstawania błędu systematycznego zależy od konkretnego rozwiązania układów elektronicznych toru pomiaru fazy [4, 225], można jednak dokonać kalibracji błędu pomiaru odległości względem amplitudy odebranego sygnału [5]. Powierzchnie różniące się właściwościami optycznymi w różnym stopniu osłabiają odbitą wiązkę światła, co daje różną amplitudę powracającego sygnału i w rezultacie różne błędy pomiaru odległości. Wiązka światła, padając na powierzchnię wykonaną z danego materiału, ulega częściowo odbiciu pod kątem równym kątowi padania, a częściowo rozproszeniu w wielu kierunkach (rys. 2.22A). Stosunek mocy wiązki odbitej do mocy wiązki padającej (zwany reflektancją zwierciadlaną) oraz stosunek mocy wiązki rozproszonej do padającej (reflektancja dyfuzyjna) zależą od wysokości i pochylenia nierówności powierzchni oraz cech samego materiału. Reflektancja dyfuzyjna ρ jest parametrem decydującym o właściwościach danej powierzchni względem dalmierza laserowego. 50

A nadajnik odbiornik wi¹zka padaj¹ca normalna do powierzchni i i odbicie zwierciadlane B wi¹zka rozproszona Rysunek 2.22. Odbicie światła i wielkość śladu optycznego w zależności od kąta padania wiązki (opis w tekście) Zwiększenie kąta padania φ i wiązki światła na powierzchnię obserwowaną powoduje istotny wzrost zarówno bezwzględnego błędu pomiaru odległości, jak i odchylenia standardowego. Odbicie światła od powierzchni przeszkód ma głównie charakter dyfuzyjny, a wzrost błędu związany jest z osłabieniem wiązki odbitej pod większym kątem (rys. 2.22A), zgodnie z prawem Lamberta: I d = I e cos φ i, (2.58) gdzie I e jest intensywnością (mocą przypadającą na jednostkowy kąt bryłowy) światła padającego, a I d intensywnością światła odbitego. Po uwzględnieniu reflektancji dyfuzyjnej I d = I e ρcos φ i. (2.59) Wpływ na błąd pomiaru ma też zwiększenie powierzchni śladu optycznego wiązki padającej pod dużym kątem (rys. 2.22B) zjawiska pochłaniania i rozpraszania światła zachodzą na większej powierzchni. Biorąc pod uwagę wzór (2.59) i to, że odbite światło jest wychwytywane przez detektor o powierzchni (aperturze) A r, znajdujący się w odległości r od obserwowanego obiektu, można określić całkowitą moc powracającego do sensora sygnału: P r = η P ea r ρcos φ i ρcos φ i πr 2 = k s r 2, (2.60) gdzie P e jest mocą emitowaną przez sensor, a η stałą zależną od właściwości odbiornika. Z powyższego wzoru widać, że moc powracającego sygnału jest odwrotnie proporcjonalna do kwadratu mierzonej odległości, a proporcjonalna do reflektancji i kosinusa kąta padania wiązki. Stała k s = ηpear π zależy wyłącznie od właściwości sensora. Powyższa zależność wraz z równaniem (2.55) stanowi model matematyczny toru pomiaru odległości skanera AMCW i pozwala na predykcję σ r, jeżeli znane są wszystkie parametry sensora oraz właściwości optyczne i geometria środowiska. Adekwatność tego modelu potwierdzają charakterystyki σ r = f(r t ) zamieszczone na rys. 2.19. Jednocześnie wyniki pomiarów odległości od czarnej, matowej powierzchni (rys. 2.20) wskazują, że predykcja σ r wyznaczona na podstawie wzoru (2.55) może dawać zbyt optymistyczne rezultaty w przypadku bardzo małych amplitud powracającego sygnału. 51

Z przeprowadzonych eksperymentów wynika, że rozrzut pomiarów odległości dokonanych w kontrolowanych warunkach daje się dobrze aproksymować rozkładem Gaussa. Podstawowe źródła zakłóceń w fotodetektorze można przedstawić jako sumę kilku rodzajów szumów elektronicznych (por. p. 2.4.1). Można wykazać [4], że amplituda odbieranego sygnału poddanego takim zakłóceniom ma rozkład Gaussa, gdy spełnione jest następujące kryterium: 1 2 V r 2 σn. 2 (2.61) Inne przyczyny niepewności pomiarów W porównaniu z przedstawionymi w podrozdziale 2.3 dalmierzami ultradźwiękowymi sensory laserowe mają korzystne cechy wynikające z właściwości światła jako medium pomiarowego. Jednak w przypadku tych sensorów także występują zjawiska powodujące całkowicie błędne odczyty odległości, czyli niepewność co do przedmiotu pomiaru. Skutkiem odbicia od powierzchni zwierciadlanej może być brak odczytu odległości, ponieważ odbity sygnał nie powraca do sensora, lub powstawanie błędnego odczytu po wielokrotnym odbiciu, podobnie jak w przypadku sonarów. Ze względu na małą długość fal świetlnych zjawiska takie w praktyce występują bardzo rzadko (jedynie dla luster) i w odniesieniu do skanera KARiI nie zostały zaobserwowane. r A A 1 A 2 B C Im z 1 + z 2 z 2 V 2 2 3 V 1 1 z 1 Re z3 V 3 z 1 + z 3 1+3 3 Rysunek 2.23. Ilustracja zjawiska mixed pixels (opis w tekście) Zjawiskiem charakterystycznym dla skanerów laserowych jest natomiast całkowicie błędny pomiar odległości w sytuacjach, gdy wysłana przez sensor wiązka laserowa pada na powierzchnię o niejednorodnych właściwościach optycznych (rys. 2.23A) lub gdy odbija się od dwóch (lub więcej) powierzchni znajdujących się w różnej odległości od sensora (rys. 2.23B). Zjawisko to, noszące w literaturze anglojęzycznej miano mixed pixels [125] lub mixed points [3], jest wynikiem uwzględnienia w odbiorniku dalmierza światła odbitego w obszarze całego śladu optycznego wiązki padającej na obserwowaną powierzchnię, który to ślad ma skończoną wielkość. Z tego względu zjawisko to będzie dalej nazywane efektem śladu optycznego. Występuje ono nie tylko w dalmierzach AMCW, jednak w tej klasie sensorów stanowi szczególnie istotny czynnik wpływający na niepewność pomiarów [4, 5, 93, 272]. Efekt śladu optycznego w skanerach laserowych używanych w robotyce został po raz pierwszy zauważony i opisany dla skanerów 3D klasy AMCW [125, 175]. We wczesnych pracach sugerowano, że jest to zjawisko właściwe metodzie AMCW, a sposobem eliminacji 52

części błędów jest uśrednianie pomiarów (w obrazie odległościowym 3D) lub filtracja medianowa [93, 125]. W pracy [4] mechanizm powstawania efektu śladu optycznego w dalmierzu AMCW został poddany szczegółowej analizie, uwzględniającej zarówno odczyt odległości, jak i amplitudy powracającego sygnału. Jeżeli dalmierz AMCW wysyła modulowany sygnał postaci V t cos(ωt), który daje ślad optyczny o powierzchni A podzielony między dwa obszary (jak na rys. 2.23) o powierzchniach odpowiednio A 1 i A 2, oddalone od siebie o r, to sygnały odbite mają postać V 1 cos(ωt + φ 1 ) i V 2 cos(ωt + φ 2 ), gdzie φ 1 i φ 2 są odpowiednimi przesunięciami fazy wynikającymi z różnych odległości obu powierzchni od sensora. Sumaryczny sygnał w odbiorniku dalmierza może być zapisany jako: V cos(ωt + φ) = V 1 cos(ωt + φ 1 ) + V 2 cos(ωt + φ 2 ), (2.62) gdzie φ jest mierzoną fazą (proporcjonalną do odległości), a V amplitudą sygnału. Wielkości te są opisane następującymi zależnościami: V = V1 2 + V 2 2 + 2V 1V 2 cos(φ 1 φ 2 ), (2.63) tgφ = V 1 sin φ 1 + V 2 sinφ 2 V 1 cos φ 1 + V 2 cos φ 2. (2.64) Można zauważyć, że przesunięcie fazowe w odbiorniku zależy nie tylko od faz obu sygnałów składowych, lecz także od stosunku amplitud tych sygnałów. Amplituda zależy od mocy powracającego sygnału, zależnej z kolei od powierzchni śladu optycznego i właściwości optycznych obiektu, na który pada dana część wiązki. W pracy [125] zaproponowano reprezentację pary (V, φ) jako liczby zespolonej z = a + bi, której moduł odpowiada amplitudzie sygnału, a argument jego fazie. Odebrany sygnał (2.62) można zapisać jako V (cos φ + sinφ) = V 1 (cos φ 1 + sinφ 1 ) + V 2 (cos φ 2 + sinφ 2 ), (2.65) pominąwszy zależność od czasu [4]. Wykorzystując postać trygonometryczną liczb zespolonych, wzór (2.65) można zapisać w postaci: V (cos φ + isin φ) = V 1 (cos φ 1 + isin φ 1 ) + V 2 (cos φ 2 + isin φ 2 ), (2.66) co odpowiada zapisowi w postaci algebraicznej: a + bi = (a 1 + a 2 ) + (b 1 + b 2 )i = z 1 + z 2. (2.67) Odebrany sygnał odpowiada sumie liczb zespolonych z 1 i z 2 reprezentujących sygnały odbite od obu rozważanych powierzchni, a zmierzona faza jest argumentem tej sumy. Posługując się graficzną interpretacją liczb zespolonych (rys. 2.23C), łatwo można zauważyć, że faza φ w odbiorniku nie zawsze będzie się znajdować w przedziale < φ 1,φ 2 >. Sytuacja taka wystąpi, gdy różnica między fazami obu sygnałów składowych przekroczy 180, co biorąc pod uwagę wzór (2.47), odpowiada różnicy odległości r > λam 4, czyli ponad połowie zakresu jednoznaczności r u. Pomiar będący wynikiem efektu śladu optycznego w powyższym przypadku jest szczególnie mylący, gdyż wynikowa faza z zakresu < φ 2,2π > wskazuje na istnienie przeszkody w odległości większej niż odległość od dalszego obiektu, natomiast z zakresu < 0,φ 1 > na przeszkodę w odległości mniejszej niż dystans do bliższego obiektu. 53

bia³y arkusz na tle styropianu czarny arkusz na tle styropianu Rysunek 2.24. Efekt śladu optycznego w pomiarach skanerem KARiI Z użyciem skanera KARiI wykonano eksperymenty, których celem było stwierdzenie, dla jakich obiektów i w jakich zakresach mierzonych odległości występuje efekt śladu optycznego. Podczas pierwszego eksperymentu w odległości 1 m od skanera ustawiono przeszkodę w postaci płyty styropianowej, do której kolejno przymocowywano na wysokości płaszczyzny skanowania arkusze wykonane z białego papieru oraz z matowej, czarnej tektury. W odległości 2 m za płytą styropianową znajdowała się biała powierzchnia ściany laboratorium. Zebrano pomiary z 30 skanów, wyznaczono średnią odległość zmierzoną i odchylenie standardowe. Wyniki eksperymentu obrazuje rys. 2.24, na którym pomiary odległości przedstawiono jako okręgi o promieniu równym ich odchyleniu standardowemu. W przypadku czarnego arkusza widoczne jest odkształcenie spowodowane większym błędem systematycznym pomiaru dla czarnej powierzchni, a także większe odchylenie standardowe. Największe odchylenie standardowe w obszarze płyty styropianowej zanotowano jednak dla punktów na granicy obszaru białego i czarnego (zaznaczone strzałkami), gdzie występuje efekt śladu optycznego. Dla białego arkusza na tle styropianu wzrost rozrzutu pomiarów na granicy obszarów nie występuje właściwości optyczne obu powierzchni są zbliżone. W obu przypadkach między płytą styropianową a ścianą (której obraz także jest zniekształcony wskutek błędu systematycznego) można zaobserwować punkty, które nie odpowiadają żadnej istniejącej przeszkodzie, jest to efekt śladu optycznego spowodowany padaniem wiązki częściowo na krawędź płyty, a częściowo na ścianę. Wraz z przemieszczaniem się wiązki skanera z płyty na ścianę fantomowe punkty widoczne są w pobliżu płyty, a następnie w pobliżu ściany wobec zbliżonych właściwości optycznych styropianu i białej, matowej ściany czynnikiem decydującym o wyniku pomiaru jest wzajemny stosunek powierzchni części śladu optycznego padających na płytę i na ścianę. Pomiary odległości powstałe w wyniku efektu śladu optycznego mają wyraźnie większe odchylenie standardowe niż pomiary prawidłowe. Do ustalenia kształtu śladu optycznego i oszacowania jego wielkości wykorzystano czarno-białą przemysłową kamerę CCD, pozwalającą na rejestrację światła w zakresie bliskiej podczerwieni. Badanie wykonano przy wyłączonym mechanizmie obrotu zwierciadła. Płytę z arkuszem papieru, na który naniesiono skalę, ustawiono prostopadle do wiązki dalmierza w odległości 1, 2 i 3 m. Uzyskane obrazy przedstawiono na rys. 2.25. Jak widać, ślad wiązki ma kształt kołowy, a jego przybliżona średnica wynosi odpowiednio: 25 mm dla odległości 1 m, 42 mm dla 2 m i 66 mm dla 3 m. Wartości te odpowiadają kątowi rozwarcia wiązki 54

r = 3 [m] r = 2 [m] r = 1 [m] 66 [mm] 42 [mm] 25 [mm] 200 [mm] 200 [mm] 200 [mm] Rysunek 2.25. Ślad optyczny wiązki skanera KARiI około 1,4. Ponieważ rozdzielczość kątowa skanera wynosi 1,286, ślady optyczne kolejnych wiązek wysyłanych podczas skanowania nakładają się na siebie w niewielkim stopniu. Kamerę CCD wykorzystano także w kolejnym eksperymencie, w którym zbadano odległości między przeszkodami, w jakich występuje efekt śladu optycznego. Pomiarów dokonano przy wyłączonym mechanizmie obrotu zwierciadła. Użyto płyty z szarego kartonu, którą umieszczono prostopadle do wiązki dalmierza, w taki sposób, by około połowa śladu optycznego znajdowała się na tej płycie (rys. 2.26). Pozostała część wiązki padała na pionowy ekran z białego kartonu, który był przemieszczany względem pierwszej płyty. Ekran ustawiano w odległości od 0,5 do 2 m od pierwszej przeszkody, w odstępach 0,5 m. W każdym z położeń ekranu wykonano 280 pomiarów odległości, następnie wyznaczono wartość średnią i odchylenie standardowe. Wykonano dwie serie pomiarów, w których pierwszy obiekt znajdował się w odległości odpowiednio 1 i 1,5 m od skanera. 160 1 czêœæ œladu optycznego na bli szym obiekcie odleg³oœæ zmierzona [cm] 150 140 130 120 110 100 r 1 = 100 [cm] r 1 = 150 [cm] odchylenie standardowe [cm] 0.95 0.9 0.85 0.8 0.75 0.7 0.65 0.6 0.55 r 1 = 150 [cm] r 1 = 100 [cm] kartonowy ekran arkusz kartonu 90 50 100 150 200 odlegloœæ miêdzy obiektami [cm] 0.5 50 100 150 200 odlegloœæ miêdzy obiektami [cm] Rysunek 2.26. Wpływ efektu śladu optycznego na dokładność pomiaru skanera KARiI Wyniki zaprezentowano na rys. 2.26 w postaci wykresów średniej odległości zmierzonej i odchylenia standardowego w funkcji odległości między przeszkodami r. Efekt śladu optycznego występuje w całym badanym zakresie odległości między dwoma powierzchniami. Przy stałym stosunku powierzchni części śladu optycznego na pierwszej i drugiej prze- 55

szkodzie oddalenie ekranu powodowało redukcję aplitudy odbitej od niego części powracającego sygnału, a co za tym idzie, mniejszy wpływ tego sygnału na pomiar odległości (zgodnie ze wzorem 2.64). Odchylenie standardowe pomiarów rosło wraz z odległością między obiektami, co także wskazuje na spadek amplitudy sumarycznego sygnału w odbiorniku. Należy zauważyć, że praktyczny zasięg zastosowanego w badanym skanerze dalmierza AMCW, wynoszący 5 m, jest mniejszy niż 1 4 λ AM. Z tego powodu nie ma możliwości odebrania sygnałów odbitych, których różnica faz daje wyniki pomiarów wskazujące fantomowe przeszkody przed bliższą powierzchnią lub za powierzchnią dalszą. 180 160 1 obiekt bli szy obiekt dalszy liczba pomiarów 140 120 100 80 2 3 4 60 40 20 0 1800 2000 2200 2400 2600 2800 3000 zmierzona odleg³oœæ [mm] Rysunek 2.27. Wpływ efektu śladu optycznego na rozrzut pomiarów skanera KARiI Z użyciem dwóch płyt kartonowych zbadano, jaki wpływ na błędy pomiaru powstające na krawędziach przeszkód ma stosunek powierzchni śladu optycznego na obu płytach. W tym przypadku płytę z szarego kartonu umieszczono 2 m od skanera, a za nią ustawiono na stałe w odległości 1 m kartonowy ekran. Następnie płytę z szarego kartonu przemieszczano w kierunku prostopadłym do wiązki pomiarowej, tak by uzyskać różny podział śladu optycznego na obu obiektach. W każdym z położeń płyty wykonano 280 pomiarów odległości. Na rysunku 2.27 w postaci histogramów przedstawiono wyniki pomiarów odległości, na które nałożono krzywe Gaussa, wykreślone dla wartości średnich i odchyleń standardowych wyznaczonych z danych pomiarowych. Pomiar oznaczony na rysunku cyfrą 1 wykonano dla śladu wiązki leżącego całkowicie na pierwszym obiekcie, pomiar 4 dla śladu wiązki leżącego na ekranie (usunięto pierwszą płytę), a pomiary 2 i 3 dla wiązki stopniowo przechodzącej z pierwszej płyty na znajdujący się za nią ekran. Widoczny jest wzrost rozrzutu pomiarów dla śladu optycznego podzielonego między oba obiekty. Kształt histogramów 2 i 3 bardziej odbiega od rozkładu normalnego niż kształt histogramów pomiarów, których nie zakłóca efekt śladu optycznego. Wyniki tego eksperymentu potwierdzają obserwację wynikającą z rys. 2.24, że pomiary odległości, w których występuje zjawisko podziału śladu optycznego, mają istotnie większy rozrzut niż pomiary prawidłowe, co wynika z redukcji amplitudy powracającego sygnału [4]. 56

Model niepewności Wyniki eksperymentów pozwalają stwierdzić, że niepewność pomiarów odległości skanerem AMCW zależy głównie od intensywności powracającego sygnału świetlnego. Wniosek ten potwierdza także analiza źródeł literaturowych opisujących badania podobnych sensorów [3, 4, 5, 168, 175, 206, 225, 237]. Dominującym rodzajem błędu jest błąd systematyczny pomiaru odległości. W literaturze [4, 5] opisane są metody skutecznej kalibracji sensorów AMCW. Metody te wymagają jednoczesnego pomiaru odległości i amplitudy sygnału V r. Pomiar ten w dalmierzu Pepperl & Fuchs, stanowiącym podstawę skanera KARiI, nie jest możliwy. Brak kanału pomiaru amplitudy sygnału istotnie utrudnia interpretację pomiarów odległości uzyskanych ze skanera KARiI i zmniejsza jego przydatność w nawigacji robota mobilnego. W związku z powyższym ograniczeniem proponowany model matematyczny skanera KARiI oparto na analizie zależności między odległością rzeczywistą, odległością zmierzoną i odchyleniem standardowym pomiaru, które to zależności otrzymano metodą eksperymentalną. Biorąc pod uwagę wyniki eksperymentów przedstawione na rys. 2.19, przyjęto, że maksymalna odległość mierzona skaneram KARiI r max = 4 m. Pomiary wykraczające poza r max są odrzucane. Na podstawie eksperymentów stwierdzono, że pomiary dla określonych kombinacji odcienia szarości powierzchni (reflektancji ρ) i kąta padania wiązki φ i stają się bezużyteczne z powodu bardzo dużego błędu bezwzględnego. Uznano, że błąd ten jest spowodowany zbyt małą intensywnością sygnału świetlnego powracającego do sensora przy danej odległości mierzonej r m oraz kombinacji kąta i reflektancji powierzchni. Pomiary wykonane w takich warunkach powinny być odrzucone ze względu na ich małą wiarygodność. Zbyt mała intensywność powracającego sygnału nie zapewnia poprawności proponowanego modelu procesu pomiarowego. Na podstawie amplitudy V r można określić przybliżoną wartość wariancji σ 2 n [4], a co za tym idzie, stwierdzić, czy spełniony jest warunek (2.61) oraz czy można zaniedbać wpływ szumu fotonowego we wzorze (2.55). Ponieważ skanerem KARiI nie można zmierzyć V r, w celu określenia, czy dany pomiar odległości spełnia warunki stosowalności modelu, wykorzystano charakterystykę σ r = f(r m ), gdyż zauważono, że nadmiernym błędom bezwzględnym pomiaru towarzyszy wzrost rozrzutu wyników pomiaru. Na podstawie analizy wyników eksperymentów w kontrolowanym środowisku za przypadek graniczny przyjęto pomiary wykonane dla powierzchni szarej o wypełnieniu 60% i kąta padania wiązki φ i = 60 (rys. 2.28A). A bezwzglêdny b³¹d pomiaru [cm] 20 0-20 -40-60 -80-100 o bia³y, i =0 o bia³y, i=60 o szary 60%, =60 i B odchylenie standardowe [cm] 16 14 12 10 8 6 4 2 o bia³y, i =0 o bia³y, i=60 o szary 60%, =60 i -120 50 100 150 200 250 300 350 400 odleg³oœæ zmierzona [cm] 0 50 100 150 200 250 300 350 400 odleg³oœæ zmierzona [cm] Rysunek 2.28. Zakres użytecznych pomiarów skanerem KARiI (opis w tekście) 57

Zależność między odchyleniem standardowym a odległością zmierzoną σ r = f(r m ) dla opisanego przypadku aproksymowano wielomianem postaci: σ r = a 0 + a 1 r m + a 2 r 2 m + a 3 r 3 m, (2.68) gdzie a i,i = 0...3, są poszukiwanymi współczynnikami. Wykorzystując pakiet Matlab, wyznaczono dopasowanie w sensie minimum sumy kwadratów odchyłek danych eksperymentalnych od przyjętego modelu. Otrzymano następujące współczynniki: a 3 = 2,054 10 7, a 2 = 4,5 10 5, a 1 = 0,017, a 0 = 2,085. Na rysunku 2.28B przedstawiono uzyskane z eksperymentu wartości σ r = f(r m ) i dopasowaną krzywą postaci (2.68) (linia ciągła). Uznano, że wszystkie pomiary, dla których wyznaczone na podstawie wystarczająco licznej próby odchylenie standardowe w funkcji wskazywanej odległości r m znajduje się powyżej krzywej (2.68), czyli w obszarze zakreskowanym na rys. 2.28B, będą odrzucane. Odrzucenie pomiarów, których odchylenie standardowe jest nadmierne, pozwala też usunąć większość punktów powstałych w wyniku efektu śladu optycznego. A wspó³czynnik korekcji k 2 0-2 -4-6 -8-10 bia³y o =40 i B bezwzglêdny b³¹d pomiaru [cm] 10 0-10 -20-30 -40-50 -60 pomiary skorygowane pomiary przed korekcj¹ -12 150 200 250 300 350 400 odleg³oœæ rzeczywista [cm] -70 50 100 150 200 250 300 350 400 odleg³oœæ rzeczywista [cm] Rysunek 2.29. Korekcja błędu systematycznego skanera KARiI metodą opisaną w pracy [139] (opis w tekście) Błąd systematyczny w torze pomiaru odległości powstaje w układzie pomiaru przesunięcia fazy i zależy od intensywności sygnału powracającego do sensora, a więc pośrednio od właściwości optycznych obserwowanej powierzchni, czyli od jej reflektancji ρ i kąta φ i między wiązką skanującą a normalną do danej powierzchni (por. wzór 2.60). Błąd ten można wyeliminować, jeżeli możliwy jest pomiar amplitudy powracającego sygnału [4]. Ponieważ w skanerze KARiI nie ma takiej możliwości, zaproponowano inne metody korekcji pomiarów odległości. W pracy [139] zaprezentowano metodę, w której wykorzystano zaobserwowane podobieństwo między zależnością błędu bezwzględnego od zmierzonej odległości a zależnością odchylenia standardowego od zmierzonej odległości. Wyniki korekcji tą metodą błędu systematycznego skanera KARiI przedstawiono na rys. 2.29B, gdzie zakreskowano obszar pomiarów, które nie podlegają korekcji. Metoda zaproponowana w pracy [139] pozwala zredukować błąd systematyczny pomiaru odległości, jej wadą jest jednak arbitralny wybór danych, na podstawie których wyznaczono współczynnik korekcyjny. Z tego powodu metoda jest skuteczna jedynie w odniesieniu do pomiarów, w których amplituda powracającego sygnału jest zbliżona do amplitudy uzyskiwanej dla referencyjnej charakterystyki odpowiadającej białej powierzchni i kątowi padania φ i = 40. 58

Wyniki kalibracji skanera AMCW opublikowane w artykule [5] wskazują, że błąd systematyczny w pomiarach odległości może być opisany jako zależny jedynie od V r : 1 1 r = a r Vr 2 + b r + c r, (2.69) V r gdzie współczynniki a r, b r i c r uzyskano metodą eksperymentalną, z wykorzystaniem pomiaru amplitudy sygnału w odbiorniku. W podrozdziale 2.4.1 pokazano z kolei, że dla dalmierza AMCW, odbierającego wystarczająco intensywny sygnał odbity od przeszkody, wariancja mierzonej odległości σ 2 r jest w przybliżeniu odwrotnie proporcjonalna do kwadratu V r (2.56). Na tej podstawie odchylenie standardowe pomiaru odległości σ r można zapisać jako funkcję V r : σ r a σr 1 V r, (2.70) gdzie współczynnik a σr uzyskuje się metodą eksperymentalną. Biorąc pod uwagę równania (2.69) 10 i (2.70), można wyznaczyć błąd bezwzględny jako funkcję odchylenia 0-10 standardowego: r = a 0 + a 1 σ r + a 2 σ 2 r, (2.71) gdzie a i,i = 0...2, są współczynnikami zależnymi od wartości nieznanych stałych a σr, a r, b r, c r. W przypadku skanera KARiI współczynników występujących w równaniach (2.69) i (2.70) nie można określić z powodu niemożności pomiaru amplitudy V r, można jednak wyznaczyć charakterystyki zależności błędu bezwzględnego od odchylenia standardowego. Na rysunku 2.30 za pomocą znaków + przedstawiono wartości bezwzglêdny b³¹d pomiaru [cm] -20-30 -40-50 -60-70 -80 0 1 2 3 4 5 6 odchylenie standardowe [cm] Rysunek 2.30. Zależność między odchyleniem standardowym a bezwzględnym błędem pomiaru skanerem KARiI r = f(σ r ) otrzymane eksperymentalnie we wszystkich pomiarach z rys. 2.19 mieszczących się w zakresie pomiarowym uprzednio określonym równaniem (2.68). Linią ciągłą zaznaczono przebieg funkcji (2.71), której współczynniki wyznaczono przez dopasowanie w sensie minimum sumy kwadratów odchyłek między danymi eksperymentalnymi a modelem: a 2 = 1,007, a 1 = 6,933, a 0 = 3,654. Linią przerywaną zaznaczono aproksymację tej samej zależności funkcją liniową, daje ona jednak znacznie gorsze dopasowanie. Skorygowana wartość odległości zmierzonej r m liczona jest na podstawie równania: r m = r m + r. (2.72) Na rysunku 2.31A liniami ciągłymi przedstawiono błędy bezwzględne pomiaru odległości z uwzględnieniem proponowanej korekcji r = r m r t dla danych z rys. 2.19, natomiast liniami przerywanymi błędy bezwzględne bez korekcji. Na rysunku 2.31B, na którym błędy bezwzględne po korekcji przedstawiono w większej skali, widać, że wartość bezwzględna 59

błędu skorygowanego pomiaru nie przekracza 2,5 cm w całym rozważanym zakresie odległości, dla wszystkich serii pomiarów wykonanych z różnymi kombinacjami ρ i φ i. Zastosowanie korekcji opartej na głębszej analizie zjawisk zachodzących w torze pomiaru odległości skanera AMCW pozwala zredukować błąd bezwzględny w większym stopniu, a co istotniejsze, dla większego zakresu warunków pomiaru niż prosta metoda opisana w pracy [139]. Najistotniejsze wydaje się tu uchwycenie w równaniu (2.71) nieliniowego charakteru zależności r = f(σ r ). A bezwzglêdny b³¹d pomiaru [cm] 10 0-10 -20-30 -40-50 -60 pomiary skorygowane pomiary przed korekcj¹ -70 50 100 150 200 250 300 350 400 odleg³oœæ rzeczywista [cm] B bezwzglêdny b³¹d pomiaru [cm] 2.5 2 1.5 1 0.5 0-0.5-1 -1.5-2 -2.5 50 100 150 200 250 300 350 400 odleg³oœæ rzeczywista [cm] Rysunek 2.31. Korekcja błędu systematycznego w pomiarach odległości skanerem KARiI (opis w tekście) Zadaniem skanera jest identyfikacja położenia obiektów w otoczeniu względem robota. Naturalnym układem współrzędnych, w którym wykonywane są pomiary, jest układ biegunowy, a parametry identyfikowanego modelu są określone przez odległość r i kąt ϕ względem układu współrzędnych robota. Odległość r jest mierzona dalmierzem, a kąt ϕ wynika z bieżącego ustawienia lustra skanera względem jego układu odniesienia, który z kolei jest jednoznacznie usytuowany względem układu związanego z robotem (rys. 2.32). Zakładając, że odczyt odległości mieści się w użytecznym zakresie, a błąd systematyczny został skorygowany, przyjęto, że niepewność r zależy od składnika losowego. Biorąc pod uwagę wyniki doświadczeń i rozważań dotyczących źródeł zakłóceń w torze pomiaru odległości skanera oraz ich propagacji aż do uzyskania σ 2 r, za model matematyczny rozrzutu wyników pomiarów odległości skanerem KARiI przyjęto rozkład normalny. Pomiar odległości może być przedstawiony jako suma prawdziwej (nieznanej) wartości pomiaru r i addytywnego błędu δr, będącego zmienną losową o zerowej wartości oczekiwanej i wariancji σ 2 r: ˆr = r + δr, δr = N(0,σ r ), (2.73) gdzie ˆr jest estymatą pomiaru, a σ r odchyleniem standardowym. Ponieważ r nie jest znane, musi być zastąpione dostępną oceną zmierzonej odległości, np. wartością średnią z serii pomiarów r m. Także odchylenie standardowe może być określone na podstawie wystarczająco licznej próby. Sterownik skanera KARiI nie pozwala na wielokrotny pomiar przy danym ustawieniu lustra odchylającego. Dlatego w celu otrzymania statystyki pomiarów odległości należy wykonać wiele kompletnych skanów, co wymaga dłuższego czasu (np. wykonanie 30 skanów 2D trwa 84 s). Gdy dysponuje się serią pomiarów odległości do tego samego punktu w otoczeniu robota, należy wyznaczyć r m i σ r, a następnie, stosując wzór (2.68), stwierdzić, czy pomiar leży w zakresie użytecznym. Błąd systematyczny 60

jest korygowany opisaną wyżej metodą (wzory 2.71 i 2.72), co pozwala przyjąć skorygowaną odległość r m za estymatę r. Jeżeli wielokrotne powtórzenia pomiaru i odczyt amplitudy powracającego sygnału nie są możliwe, to błąd systematyczny nie może być skorygowany. W przypadku skanera KARiI oznacza to, że pomiary odległości są użyteczne jedynie w zakresie do około 2 m, gdy pomiary dotyczą obiektów o dużej reflektancji (jasne, gładkie powierzchnie) i są wykonywane pod niewielkim kątem. W takiej sytuacji należy uznać, że r jest bieżącym odczytem odległości. Jeżeli znana jest amplituda V r, to wariancja pomiaru odległości sensora AMCW może być wyznaczona za pomocą wzoru (2.55) lub przybliżonej zależności (2.56), bez konieczności powtarzania pomiarów. Jeżeli nie ma możliwości pomiaru amplitudy, lecz znany jest kąt padania wiązki na przeszkodę i jej właściwości optyczne, to w celu wyznaczenia σ r można się posłużyć modelem matematycznym toru pomiaru odległości. Postać modelu zależy od założeń dotyczących źródeł zakłóceń pomiaru. Wykorzystując równanie (2.55) i zakładając, że amplituda V r jest wprost proporcjonalna do odbieranej mocy sygnału (2.60), otrzymuje się następującą zależność: σ 2 r = ( ) 2 ( λam σ n r 2 4π k s ρcos φ i ) 2 + ( ) 2 λam k p r 2 + σ 4π k s ρcos φ e. 2 (2.74) i Przyjąwszy założenie, że najistotniejszym źródłem zakłóceń w dalmierzu AMCW jest szum fotonowy, w pracy [225] oszacowano odchylenie standardowe mierzonej odległości jako σ r λ AM 2 2πSNR, (2.75) gdzie SNR jest stosunkiem sygnału użytecznego do szumu (ang. signal-to-noise ratio) w detektorze. Następnie, zakładając, że emisja fotonów w detektorze jest procesem Poissona, wyznaczono SNR jako funkcję odległości mierzonej i warunków pomiaru: ρcos φi SNR = k SNR, (2.76) r gdzie k SNR jest stałą zależną od parametrów sensora. Na podstawie wzorów (2.75) i (2.76) można zbudować równanie wyrażające spodziewaną zależność odchylenia standardowego od r, ρ oraz φ i : σ r λ AM 2 2πk SNR r ρcos φi. (2.77) Jak wynika ze wzoru (2.77), w modelu zaproponowanym przez Nitzana i współautorów [225], a wykorzystywanym także w innych pracach [93, 125], przewidziano liniową zależność σ r od zmierzonej odległości, która to zależność nie znajduje potwierdzenia w rezultatach pomiarów skanerem KARiI. Z wykresów na rys. 2.19 wynika, że odchylenie standardowe jest proporcjonalne do kwadratu mierzonej odległości i odwrotnie proporcjonalne do kosinusa kąta padania wiązki, co pozostaje w zgodzie z modelem opisanym równaniem (2.74). Model odpowiadający zależności (2.77) można otrzymać z równania (2.74), uwzględniając jedynie jego drugi składnik, opisujący wpływ szumu fotonowego. Aktywne sensory optyczne charakteryzują się wąską wiązką pomiarową o niewielkiej rozbieżności. W związku z tym, a także w związku z możliwością precyzyjnego pomiaru kąta odchylenia wiązki skanera (stosowane są przetworniki obrotowo-impulsowe lub tarcze kodowe o dużej rozdzielczości) w wielu pracach dotyczących modelowania skanerów laserowych 61

rozk³ad jednostajny oœ wi¹zki Y skan Y rob KARI < < r X skan obrotowe zwierciad³o skan X rob Rysunek 2.32. Graficzna interpretacja modelu niepewności pomiarów skanerem KARiI 2D [3, 4] i przetwarzania pochodzących z nich danych [23, 141] zaniedbywana jest niepewność pomiaru kąta między osią odchylonej wiązki skanującej a układem odniesienia skanera. Wyniki eksperymentów ze skanerem KARiI ujawniły stosunkowo dużą rozbieżność wiązki i znaczną wielkość śladu optycznego. Cechy te wynikają z użycia dalmierza z nadajnikiem w postaci diody LED. Ponadto, lustro odchylające napędzane jest silnikiem krokowym, wobec czego nie zastosowano czujnika przemieszczenia kątowego. Czynnikiem określającym niepewność kąta ϕ jest liczba kroków silnika, równa rozdzielczości kątowej skanu 2D. Ponieważ podczas eksploatacji skanera KARiI nie zaobserwowano zjawisk mogących być przyczyną istotnych, dominujących błędów kąta ϕ (np. gubienie kroków silnika, luzy mechaniczne), przyjęto, że wartość kąta odchylenia wiązki może być przedstawiona jako suma prawdziwej (nieznanej) wartości ϕ i addytywnego błędu δϕ, będącego zmienną losową o zerowej wartości oczekiwanej i wariancji σ 2 ϕ: ˆϕ = ϕ + δϕ, δϕ = N(0,σ ϕ ), (2.78) gdzie ˆϕ jest estymatą kąta odchylenia wiązki, a σ ϕ odchyleniem standardowym. Rozdzielczość kątowa skanera ϕ = 1,286. Wartość σ ϕ wyznaczono, zakładając, że kąt 1 2 ϕ stanowi niepewność graniczną, która zgodnie z ogólnie przyjętą zasadą [315] odpowiada niepewności całkowitej 3σ, czyli 99,73-procentowemu prawdopodobieństwu wystąpienia wartości zmiennej ϕ w przedziale ˆϕ ± 1 2 ϕ. Zgodnie z tym założeniem σ ϕ = ϕ 6. (2.79) Z zależności (2.79) wyznaczono σ ϕ = 0,214. Za estymatę kąta odchylenia wiązki przyjmowana jest jego wartość nominalna wynikająca z kolejnego indeksu pomiaru w skanie 2D. Wielkość śladu optycznego w zależności od odległości określono eksperymentalnie (rys. 2.25), a wynikający z tego kąt rozbieżności wiązki θ KARiI 1,4. Rzeczywisty rozkład mocy emitowanej na powierzchni śladu optycznego zależy od właściwości użytej diody i układu optycznego nadajnika [13, 314]. Wobec braku możliwości pomiaru tego parametru, 62

opierając się na wizualnej ocenie śladu optycznego, przyjęto, że rozkład mocy emitowanej jest równomierny. Jeżeli wiązka oświetla dwie powierzchnie znajdujące się w różnej odległości, to występuje efekt śladu optycznego, omówiony w poprzednim podrozdziale. Jeżeli natomiast pomiar został uznany za prawidłowy, to należy założyć, że wiązka pada na jednolitą powierzchnię. Cały ślad optyczny przyczynia się wówczas z jednakowym prawdopodobieństwem do uzyskania wyniku pomiaru. Niepewność wynikająca z rozbieżności kątowej wiązki nie wpływa bezpośrednio na identyfikację parametrów [r ϕ] T obserwowanego punktu, gdyż założono (w przeciwieństwie do dalmierza ultradźwiękowego), że pomiar odległości jest zawsze wykonywany wzdłuż osi wiązki, tak więc kąt ϕ dany jest wzorem (2.78). Prawdopodobieństwo istnienia przeszkody w obszarze wiązki ˆϕ 1 2 θ KARiI, ˆϕ + 1 2 θ KARiI jest jednakowe, a jego modelem jest rozkład jednostajny (prostokątny): ( p(ˆϕ) U θ KARiI, θ KARiI 2 2 ), (2.80) gdzie p( ˆϕ) jest gęstością prawdopodobieństwa. Na rysunku 2.32 przedstawiono graficzną interpretację modelu niepewności pojedynczego pomiaru w skanie sensora KARiI. 2.4.3. Charakterystyka skanera laserowego Sick LMS 200 Budowa skanera LMS 200 Komercyjny skaner laserowy typu LMS 200-30106 produkcji Sick AG jest przeznaczony do precyzyjnych pomiarów odległości, określania wymiarów obiektów i monitorowania obszarów [263]. Funkcja pomiaru odległości pozwala otrzymać dokładne i wiarygodne dane, które mogą być wykorzystane do budowy mapy otoczenia, samolokalizacji oraz wspomagania innych zadań nawigacyjnych. Skanery Sick LMS (oraz starszy model PLS) są obecnie często wykorzystywane w badaniach dotyczących robotów autonomicznych (np. [167, 267, 316, 322]). ODBIORNIK NADAJNIK r WIRUJ CE ZWIERCIAD O Rysunek 2.33. Skaner Sick LMS 200 Rysunek 2.34. Schemat budowy skanera LMS Jest to skaner klasy TOF, mierzący czas przebiegu impulsu laserowego do przeszkody i z powrotem i określający na tej podstawie odległość [263]. Sick LMS 200 jest urządzeniem 63

komercyjnym, a jego wewnętrzna struktura nie jest dostępna dla użytkownika, w związku z czym nie istniała możliwość dokładnego poznania budowy części elektronicznej sensora. Jednak w niektórych źródłach literaturowych [89] jest opisany przypuszczalny sposób działania skanerów rodziny PLS/LMS. Dioda laserowa o mocy 1 1 mw, pracująca w zakresie podczerwieni (905 nm), generuje impuls świetlny o czasie trwania 3,5 ns. W tym momencie uruchamiany jest licznik. W modelu PLS-101 ma on rozdzielczość 330 ps [89], co daje rozdzielczość pomiaru 50 mm. W przypadku skanera LMS 200 deklarowana przez producenta rozdzielczość wynosi 10 mm, co pozwala sądzić, że rozdzielczość licznika w nowszym modelu LMS jest nie mniejsza niż 66 ps. Odbity impuls trafia poprzez zwierciadło na fotodetektor. Sygnał na jego wyjściu jest porównywany z wartością progową, będącą wielokrotnością średniego poziomu szumów w fotodetektorze. Gdy amplituda powracającego sygnału przekroczy ustalony próg, licznik czasu jest zatrzymywany, a zmierzona odległość wyliczana ze wzoru (2.57). Powracające impulsy światła o różnej intensywności generują w fotodetektorze sygnały elektryczne o różnym czasie narastania, co z kolei może prowadzić do błędów w określeniu czasu powrotu impulsu, a tym samym błędów w pomiarze odległości. W skanerach Sick LMS i PLS zjawisko to jest częściowo kompensowane przez układ mikroprocesorowy rozróżniający 6 poziomów sygnału w odbiorniku [89]. Układ odchylania wiązki z nachylonym pod kątem 45 zwierciadłem (rys. 2.34) wirującym z prędkością 75 obr/s pozwala na skanowanie w płaszczyźnie poziomej i w zakresie 180. Sick LMS 200 jest sensorem programowalnym. Komunikację z urządzeniem poprzez łącze szeregowe umożliwia zestaw poleceń (tzw. telegramów), pozwalających na wybranie zakresu pomiarów odległości (zasięgu i rozdzielczości), rozdzielczości kątowej oraz zakresu kąta skanowania [262]. Fizyczna rozdzielczość kątowa skanera jest stała i wynosi 1. Skany o większej rozdzielczości uzyskuje się przez dwukrotny lub czterokrotny obrót zwierciadła w całym zakresie kąta skanowania. Przetwarzanie danych w samym sensorze umożliwia uśrednianie wyników z kilku serii pomiarów przed przesłaniem ich do komputera nadrzędnego. Wartości zmierzone mogą być odczytane za pośrednictwem łącza szeregowego. Skaner LMS 200 zamontowany na robocie TRC Labmate jest podłączony do komputera pokładowego za pomocą standardowego portu RS232C, co umożliwia komunikację z prędkością do 38 400 bps. Niepewność pomiaru odległości w kontrolowanym środowisku Przeprowadzono eksperymenty mające na celu określenie właściwości skanera LMS 200 istotnych w nawigacji robotów mobilnych 2 oraz porównanie jego właściwości ze stosowanym wcześniej skanerem opartym na dalmierzu AMCW Pepperl & Fuchs. Wykonano badania dokładności pomiaru odległości w kontrolowanym środowisku, analogiczne do przedstawionych uprzednio dla skanera KARiI. Wykorzystano te same karty o różnych stopniach szarości, a w eksperymentach przyjęto te same zakresy kątów padania wiązki i odległości. Powodem ograniczenia części badań do zakresu 4 m, pomimo znacznie większego zasięgu sensora TOF, były wymiary pomieszczenia laboratorium. Podczas testów stosowano rozdzielczość 0,5, jednak odczytywano tylko wartość pojedynczego, środkowego pomiaru odległości w skanie. Zakres mierzonej odległości ustawiono na 8,1 m. Na podstawie otrzymanych pomiarów wyliczono wartość średnią r, błąd bezwzględny r = r r t i odchylenie standardowe σ r pomiaru. Otrzymane wartości przedstawiono na wykresach w funkcji odległości rzeczy- 1 Wartość deklarowana przez producenta. 2 Dane istotne w zastosowaniach przemysłowych można znaleźć w dokumentacji urządzenia [263]. 64

wistej r t. Wyznaczono w ten sposób charakterystykę przetwarzania toru pomiaru odległości r = f(r t ) oraz zależność błędu bezwzględnego i odchylenia standardowego od odległości. A wartoœæ zmierzona [cm] B 400 350 300 250 200 150 100 50 0 50 100 150 200 250 300 350 400 odleg³oœæ rzeczywista [cm] 1 0.5 D wartoœæ zmierzona [cm] E 400 350 300 250 200 150 100 50 0 50 100 150 200 250 300 350 400 odleg³oœæ rzeczywista [cm] 1 0.5 b³¹d bezwzglêdny [cm] 0-0.5-1 -1.5-2 -2.5 b³¹d bezwzglêdny [cm] 0-0.5-1 -1.5-2 -3 50 100 150 200 250 300 350 400 odleg³oœæ rzeczywista [cm] -2.5 50 100 150 200 250 300 350 400 odleg³oœæ rzeczywista [cm] C odchylenie standardowe [cm] 0.35 0.3 0.25 0.2 0.15 0.1 0.05 0 50 100 150 200 250 300 350 400 odleg³oœæ rzeczywista [cm] Bia³y Szary 20% Szary 40% Szary 60% Szary 80% F odchylenie standardowe [cm] 0.45 0.4 0.35 0.3 0.25 0.2 0.15 0.1 0.05 0 50 100 150 200 250 300 350 400 odleg³oœæ rzeczywista [cm] 0 o 20 o 40 o 60 o Rysunek 2.35. Wyniki badania skanera LMS 200 w kontrolowanym środowisku (opis w tekście) Na rysunkach 2.35A, B i C przedstawiono wyniki pomiarów skanerem LMS 200 uzyskane dla powierzchni w różnym stopniu odbijających światło. Zróżnicowanie wyników w zależności od stopnia szarości powierzchni jest bardzo niewielkie. Charakterystyki przetwarzania pokrywają się dla wszystkich badanych powierzchni. Dla ciemniejszych powierzchni błędy bezwzględne rosną, co widać na rys. 2.35B, jednak różnica między wartością średnią błędu 65

pomiaru dla powierzchni białej i zaczernionej w 80% nie przekracza 20 mm. Dla powierzchni białej błąd bezwzględny mieści się w granicach ± 15 mm podanych przez producenta jako maksymalny błąd systematyczny. Błąd bezwzględny dla powierzchni szarej w 20% i 40% także mieści się w tych granicach dla prawie całego badanego zakresu. W przypadku skanera LMS 200 nie można zaobserwować wyraźnego wpływu stopnia szarości powierzchni na rozrzut wyników pomiarów, a odchylenie standardowe nie przekracza wartości 5 mm podanej przez producenta jako maksymalny błąd przypadkowy (rys. 2.35C). Na rysunkach 2.35D, E i F przedstawiono wyniki uzyskane dla różnych kątów padania wiązki na białą powierzchnię. Jak widać na rys. 2.35D, funkcja przetwarzania skanera LMS 200 może być dobrze przybliżona funkcją liniową dla wszystkich kątów padania wiązki na powierzchnię przeszkody. Wartości średnie z pomiarów dla różnych kątów φ i prawie się pokrywają. Z charakterystyki błędu bezwzględnego (rys. 2.35E) widać, że im większy kąt φ i, tym większy błąd bezwzględny, jednak nawet dla kąta φ i = 60 r nie przekracza 20 mm w całym badanym zakresie odległości. W przeciwieństwie do dalmierza AMCW rozrzut wartości pomiaru dla sensora LMS 200 nie jest funkcją kąta padania wiązki (rys. 2.35F). Ponieważ opisane wyżej eksperymenty nie pozwoliły określić właściwości skanera LMS 200 w pełnym zakresie mierzonych odległości, wykonano dodatkowe eksperymenty poza laboratorium w pełnym zakresie pomiaru odległości, tj. do 8 m. W porównaniu z poprzednim eksperymentem istotne było także oszacowanie wpływu materiału, z którego są wykonane przedmioty w otoczeniu skanera, na dokładność pomiarów. Te materiały to: biały karton, karton w kolorze brązowym, czarna, matowa tektura, folia aluminiowa. Sporządzono wykresy średniego, bezwzględnego błędu pomiaru odległości oraz odchylenia standardowego dla serii 100 pomiarów (rys. 2.36). 8000 10 25 A B bia³y karton C 7000 5 br¹zowy karton czarna tektura 6000 0 folia aluminiowa 20 5000-5 15 4000-10 3000-15 10 2000-20 5 1000-25 odleg³oœæ zmierzona [mm] bezwzglêdny b³¹d pomiaru [mm] bia³y karton br¹zowy karton czarna tektura folia aluminiowa 0-30 0 1000 2000 3000 4000 5000 6000 7000 8000 0 0 1000 2000 3000 4000 5000 6000 7000 8000 0 1000 2000 3000 4000 5000 6000 7000 8000 odleg³oœæ rzeczywista [mm] odleg³oœæ rzeczywista [mm] odleg³oœæ rzeczywista [mm] odchylenie standardowe [mm] Rysunek 2.36. Wyniki badania skanera Sick LMS 200 w zakresie do 8 m (opis w tekście) Błędy pomiaru dla białego, brązowego i czarnego kartonu różnią się w niewielkim stopniu. Potwierdza to małą wrażliwość skanera LMS 200 na stopień pochłaniania światła przez obserwowany obiekt. Dla powierzchni silnie odbijającej światło (folii aluminiowej) można zauważyć, że gdy odległość była większa niż około 5 m, wystąpiły wyraźnie większe błędy (rys. 2.35B). Na rysunku 2.35C można zaobserwować zależność odchylenia standardowego od rodzaju obserwowanej powierzchni. Różnice dla badanych materiałów są niewielkie z wyjątkiem powierzchni silnie odbijającej folii aluminiowej. Rozrzut wyników pomiarów wyrażony jako odchylenie standardowe w niewielkim stopniu zależy od mierzonej odległości, wzrost rozrzutu można zaobserwować dla pomiarów odległości większych niż około 7 m. Także stopień pochłaniania światła przez obserwowaną powierzchnię nie ma istotnego wpływu na rozrzut wyników. Jedynie dla folii aluminiowej zaobserwowano duże wartości odchylenia standardowego w niektórych pomiarach, szczególnie gdy dystans wynosił około 5 m. 66

W tym przypadku nie można wykluczyć wpływu lokalnych nierówności arkusza folii na wyniki pomiarów. liczba pomiarów odleg³oœci 200 180 160 140 120 100 80 60 40 20 200 200 A 180 B 180 C liczba pomiarów odleg³oœci 160 140 120 100 80 60 40 20 liczba pomiarów odleg³oœci 160 140 120 100 80 60 40 20 0 991 992 993 994 995 996 997 998 999 1000100110021003 zmierzona odleg³oœæ [mm] 0 2990 2992 2994 2996 2998 3000 3002 zmierzona odleg³oœæ [mm] 0 4990 4992 4994 4996 4998 5000 5002 zmierzona odleg³oœæ [mm] liczba pomiarów odleg³oœci 200 180 160 140 120 100 80 60 40 20 D 180 180 E F liczba pomiarów odleg³oœci 200 160 140 120 100 80 60 40 20 liczba pomiarów odleg³oœci 200 160 140 120 100 80 60 40 20 0 988 990 992 994 996 998 zmierzona odleg³oœæ [mm] 0 2981 2983 2985 2987 2989 2991 2993 zmierzona odleg³oœæ [mm] 0 4970 4975 4980 4985 4990 4995 5000 5005 zmierzona odleg³oœæ [mm] Rysunek 2.37. Histogramy pomiarów odległości skanera LMS 200 dla powierzchni białej (A, B, C) i czarnej (D, E, F) Inne przyczyny niepewności pomiarów Jak wykazały opisane wyżej eksperymenty, skaner laserowy Sick LMS 200, w którym wykorzystano impulsową metodę pomiaru czasu przebiegu światła, charakteryzuje się znacząco lepszymi właściwościami niż badany sensor AMCW. Jednak w trakcie pomiaru skanerem LMS 200 również występują zjawiska będące przyczyną niepewności co do przedmiotu pomiaru. Zaobserwowano zjawisko nieprawidłowych pomiarów odległości od powierzchni błyszczącej (zwierciadlanej), gdy powierzchnia ta nie jest ustawiona prostopadle do osi wiązki skanera. W takich sytuacjach odczyty odległości były znacznie większe niż rzeczywisty dystans między skanerem a obserwowanym obiektem. Zjawisko pomiaru z odbiciem występuje jedynie w przypadku lustra lub polerowanego metalu. Na podstawie obserwacji jakościowych można stwierdzić, że podczas pomiarów skanerem laserowym zjawisko to występuje bardzo rzadko. Dostępne w literaturze wyniki badań sensorów z rodziny Sick LMS/PLS [89, 314, 328] i innych skanerów laserowych TOF [226, 245] oraz opracowania dotyczące pomiarów odległości metodą impulsową w przemyśle [191] wskazują na występowanie efektu śladu optycznego także w skanerze LMS 200. W związku z tym podjęto próbę eksperymentalnego ustalenia, dla jakich typów powierzchni i dla jakich odległości między nimi występuje efekt śladu optycznego podczas pomiarów tym sensorem. Podobnie jak w badaniach skanera KARiI, w odległości 1 m od skanera LMS 200 ustawiono płytę styropianową, do której przymocowano na wysokości płaszczyzny skanowania arkusz białego papieru, a następnie arkusz czarnej tektury. W odległości 2 m za tą przeszkodą znajdowała się ściana laboratorium. Zebrano 67

bia³y arkusz na tle styropianu czarny arkusz na tle styropianu Rysunek 2.38. Badanie zjawiska mixed pixels w pomiarach skanerem LMS 200 pomiary z 30 skanów, wyznaczono średnią odległość zmierzoną i odchylenie standardowe. Wyniki przedstawiono na rys. 2.38 w postaci okręgów o promieniu równym odchyleniu standardowemu zmierzonej odległości. W przeciwieństwie do skanera AMCW, w przypadku pomiarów sensorem LMS 200 nie zwiększa się rozrzut wyników dla wiązki padającej na czarną powierzchnię. Nie zaobserwowano także efektu śladu optycznego w punktach na granicy obszaru białego i czarnego. Między płytą styropianową a ścianą nie można wyróżnić punktów wskazujących na pojawienie się efektu śladu optycznego. Eksperyment powtórzono, redukując odległość między płytą styropianową a ścianą do 1 m. Po tej zmianie pojawiły się pojedyncze punkty pomiędzy krawędziami płyty styropianowej a ścianą. r = 30 cm r = 50 cm r = 100 cm r = 120 cm r = 150 cm r = 200 cm Rysunek 2.39. Zakres występowania efektu śladu optycznego w pomiarach LMS 200 Aby dokładniej określić zakres odległości między obiektami, w którym występuje efekt śladu optycznego w skanerze LMS 200, przeprowadzono kolejny eksperyment. Płyta styropianowa była umieszczona w odległości 1 m od skanera, prostopadle do środkowej wiązki dalmierza (wiązka o indeksie 179), tak by część śladu optycznego znajdowała się na tej płycie, a część padała na umieszczony za nią biały, kartonowy ekran, przemieszczany względem pierwszej płyty. Utrudnieniem w przygotowaniu tego eksperymentu był brak możliwości wizualnej kontroli śladu optycznego, gdyż kamera CCD używana w eksperymentach z sensorem KARiI nie wykrywała przesuniętego w kierunku podczerwieni promieniowania skanera 68

LMS 200. Ekran ustawiano w sześciu kolejnych pozycjach, tj. 30, 50, 100, 120, 150 i 200 cm za pierwszą przeszkodą, wykonywano po 300 pomiarów odległości i wyznaczano wartość średnią oraz odchylenie standardowe. Wyniki zaprezentowano na rys. 2.39. Jak widać, efekt śladu optycznego wystąpił jedynie dla odległości do 120 cm włącznie. Gdy odległość między obiektami r była równa 150 i 200 cm, wskazanie sensora odpowiadało rzeczywistej odległości od pierwszego obiektu. Widoczne jest większe odchylenie standardowe pomiarów zakłóconych przez efekt śladu optycznego. 150 5 liczba pomiarów 100 50 1 2 3 4 0 950 1000 1050 1100 1150 1200 1250 1300 1350 1400 zmierzona odleg³oœæ [mm] Rysunek 2.40. Wpływ efektu śladu optycznego na rozrzut pomiarów skanerem LMS 200 Na dokładniejsze określenie rozrzutu wyników uzyskanych w powyższym eksperymencie pozwalają histogramy pomiarów odległości przedstawione na rys. 2.40, na które nałożono krzywe Gaussa, wykreślone dla uzyskanych w tym eksperymencie wartości średnich i wartości odchylenia standardowego. Pomiary oznaczone na rysunku cyframi od 1 do 5 wykonano dla wartości r równych odpowiednio 30, 50, 100, 120 i 200 cm. Wartości średnie pomiarów, w których wystąpił efekt śladu optycznego, są zbliżone do siebie. Odchylenie standardowe tych pomiarów pozostaje w przybliżeniu stałe, nie zaobserwowano jego zależności od odległości między obiektami. Rozrzut wartości pomiarów w widoczny sposób odbiega jednak od rozkładu normalnego w histogramach występują maksima lokalne. Odchylenie standardowe wyników serii pomiarów, w których nie występuje efekt śladu optycznego (krzywa nr 5), jest mniejsze niż wyników w pozostałych seriach. Zjawiska zaobserwowane w opisanym eksperymencie można wyjaśnić, odwołując się do dokładniejszego opisu zjawisk zachodzących podczas pomiaru odległości dalmierzem laserowym, w którym wykorzystuje się bezpośredni pomiar czasu przebiegu światła. W pracy [191] przedstawiono model symulacyjny dalmierza TOF oparty na założeniu, że obszar padania wiązki (ślad optyczny) składa się z dużej liczby elementarnych obszarów. Jeżeli obszary te są wystarczająco małe, to można założyć, że mają one homogeniczne właściwości optyczne, a odbite od nich części sygnału dalmierza zachowują w dziedzinie czasu kształt impulsu nadawanego. Impulsy odbite od elementarnych obszarów śladu optycznego są sumowane w odbiorniku, tworząc razem impuls odbierany. Moment powrotu tego sumarycznego impulsu jest wykrywany przez dalmierz. Różnice w odległości od sensora poszczególnych 69

elementarnych obszarów tworzących powierzchnię śladu optycznego powodują różnice czasu powrotu poszczególnych impulsów do odbiornika. Amplitudy powracających impulsów ulegają zmianie na skutek różnych właściwości optycznych poszczególnych obszarów. Zjawiska te determinują kształt sumarycznego impulsu wchodzącego do układu pomiaru czasu powrotu. Zmiana kształtu powracającego impulsu jest przyczyną błędów w ustaleniu czasu jego powrotu, a w konsekwencji błędów w pomiarze odległości. Należy zauważyć, że to, jaki wpływ na błąd pomiaru ma dany rodzaj odkształcenia impulsu, zależy od zastosowanej w konkretnym dalmierzu laserowym metody dokładnego pomiaru czasu powrotu impulsu [191]. Układ pomiaru czasu dalmierza Sick odbiera jako prawidłowe jedynie powracające impulsy o odpowiedniej szerokości. Jeżeli powracający impuls trwa znacznie dłużej niż impuls wysłany, to nie spełni on warunku zatrzymania licznika czasu. Z tego powodu błędne pomiary, będące wynikiem podziału śladu optycznego między dwa obiekty, występują wówczas, gdy odległość między tymi obiektami nie jest znacząco większa od szerokości nadawanego impulsu. Szerokość ta wynika z czasu trwania impulsu i dla badanego skanera LMS 200 wynosi 1,05 m. Podobne wnioski co do zakresu odległości między przeszkodami, w którym występuje efekt śladu optycznego, dla skanera Sick LMS 200 przedstawiono w pracy [329], przyczyny tego zjawiska nie zostały jednak wyjaśnione na gruncie analizy zasady pomiaru. Model niepewności Przedstawione wyniki eksperymentów prowadzą do stwierdzenia, że skaner laserowy Sick LMS 200 pozwala uzyskiwać dokładne pomiary odległości dla dystansów obejmujących prawie cały zakres pomiarowy deklarowany przez producenta. Błąd systematyczny pomiaru odległości jest niewielki i w małym stopniu zależy od reflektancji obserwowanych powierzchni, chociaż w niektórych pomiarach przekracza deklarowaną przez producenta wartość. W pomiarach odległości od powierzchni błyszczących występują sporadycznie większe błędy bezwzględne. Błąd systematyczny zależy od odległości zmierzonej. Wyniki przedstawione na rys. 2.35 wskazują, że błąd systematyczny jest zależny także od kąta padania wiązki laserowej na obserwowaną powierzchnię. Błędy przypadkowe nie przekraczają podawanej przez producenta wartości 5 mm w zakresie od około 0,5 m do 7 m, niezależnie od reflektancji obserwowanej powierzchni. Wyniki eksperymentu przedstawione na rys. 2.35 nie wykazują zależności między wartością błędów przypadkowych a kątem padania wiązki laserowej na obserwowaną powierzchnię. Proponowany dla skanera Sick LMS 200 model niepewności pomiaru jest oparty na analizie zależności między odległością rzeczywistą a odległością zmierzoną oraz odległością rzeczywistą a odchyleniem standardowym pomiaru, które to zależności otrzymano metodą eksperymentalną. Model ten dotyczy skanera pracującego w trybie zasięgu do 8,1 m i skanowania z rozdzielczością kątową 0,5. W trybie tym wykonano wszystkie eksperymenty, jest to także jedyny tryb pracy wykorzystywany w oprogramowaniu nawigacyjnym robota TRC Labmate. Biorąc pod uwagę wyniki eksperymentów, przyjęto, że maksymalna odległość mierzona skanerem LMS 200 r max = 7 m, pomiary wykraczające poza tę wartość są odrzucane. Odrębnego potraktowania wymagają błędne pomiary spowodowane efektem śladu optycznego lub odbiciem od powierzchni błyszczącej proponowany model niepewności pomiarów nie obejmuje tych błędów. Błąd systematyczny w torze pomiaru odległości powstaje głównie wskutek zaburzenia kształtu powracającego impulsu, co z kolei powoduje błąd w precyzyjnym określeniu cza- 70

0 bezwzglêdny b³¹d pomiaru [mm] -5-10 -15-20 -25 0 1000 2000 3000 4000 5000 6000 7000 8000 odleg³oœæ zmierzona [mm] Rysunek 2.41. Aproksymacja funkcją (2.82) zależności między mierzoną odległością a bezwzględnym błędem pomiaru dla LMS 200 su powrotu tego impulsu. Błąd ten jest kompensowany przez sterownik mikroprocesorowy skanera, jednak przedstawione wyniki eksperymentów wskazują, że kompensacja ta nie jest wystarczająca w całym zakresie użytecznym pomiarów. W pracy [328] zaproponowano korekcję błędu systematycznego w pomiarach odległości skanerem LMS 200 za pomocą funkcji liniowej postaci: r m = k r m + b, (2.81) której współczynniki wyznaczane są metodą najmniejszych kwadratów. Zastosowanie funkcji (2.81) nie dało jednak zadowalających wyników w odniesieniu do danych otrzymanych w przedstawionych eksperymentach (por. rys. 2.42A). Z tego powodu zaproponowano metodę korekcji pomiarów odległości opartą na otrzymanej eksperymentalnie zależności błędu bezwzględnego od zmierzonej odległości. Na rysunku 2.41 dla pomiarów z rys. 2.36 uznanych za prawidłowe (pominięto pomiary odległości do folii aluminiowej) przedstawiono w postaci znaków + wartości r = f( r m ) otrzymane eksperymentalnie. Linią ciągłą zaznaczono przebieg funkcji postaci (2.82): r = a 0 + a 1 r m + a 2 r 2 m, (2.82) której współczynniki a i,i = 0...2, wyznaczono przez dopasowanie w sensie minimum sumy kwadratów odchyłek między danymi eksperymentalnymi a modelem. Otrzymano następujący wynik: a 2 = 6,68 10 7, a 1 = 5,77 10 3, a 0 = 16,864. Wielomian (2.82) zapewniał najmniejszy błąd dopasowania spośród kilku rozważanych funkcji aproksymujących. Skorygowaną wartość odległości zmierzonej r m liczono na podstawie równania (2.72). Na rysunku 2.42A dla danych z rys. 2.36 przedstawiono błędy bezwzględne pomiaru odległości z uwzględnieniem proponowanej korekcji r = r m r t (linie ciągłe) oraz z korekcją wyznaczoną metodą podaną w pracy [328] (linie przerywane). Na rysunku 2.42B, na którym przedstawiono jedynie błędy bezwzględne po korekcji, widać, że wartość bezwzględna błędu skorygowanego pomiaru w całym rozważanym zakresie nie przekracza 6 mm dla wszystkich trzech serii pomiarów wykonanych przy różnych wartościach reflektancji powierzchni. 71

A 10 5 B 6 4 bezwzglêdny b³¹d pomiaru [mm] 0-5 -10-15 -20 bez korekcji korekcja liniowa korekcja wielomianowa bezwzglêdny b³¹d pomiaru [mm] 2 0-2 -4-6 bia³y karton br¹zowy karton czarna tektura -25 0 1000 2000 3000 4000 5000 6000 7000 8000 odleg³oœæ rzeczywista [mm] -8 0 1000 2000 3000 4000 5000 6000 7000 8000 odlegloœæ rzeczywista [mm] Rysunek 2.42. Korekcja błędu systematycznego w pomiarach odległości skanerem LMS 200 (opis w tekście) Przedstawiona procedura korekcji błędu systematycznego w pomiarach odległości nie uwzględnia błędów powstających, gdy wiązka skanera pada na obserwowaną powierzchnię pod kątem innym niż kąt prosty (gdy φ i 0). Na istnienie takich błędów wskazują wyniki eksperymentu w kontrolowanym środowisku przedstawione na rys. 2.35E. Błąd systematyczny zależny od kąta padania wiązki został uwzględniony w niektórych pracach dotyczących wykorzystania skanerów Sick PLS i LMS [89, 234]. Autorzy pracy [328] stwierdzili istnienie błędu systematycznego zależnego od kąta padania wiązki. Według nich kąt ten ma większy wpływ na wynik pomiaru niż poziom szarości obserwowanej powierzchni, co jednak nie zostało uwzględnione w proponowanym przez nich modelu sensora i metodzie kalibracji. Za błąd systematyczny r zależny od kąta φ i między osią wiązki a normalną do powierzchni uznano bezwzględny błąd pomiaru odległości występujący w pomiarach dokonanych przy φ i 0, w których błąd r zależny od mierzonej odległości został już skorygowany za pomocą zależności (2.82). Kalibracji błędu systematycznego zależnego od kąta padania wiązki dokonano na podstawie danych przedstawionych na rys. 2.35E, zebranych w kontrolowanym środowisku w zakresie odległości do 4 m. Zakładając, że błąd r zależny od kąta φ i jest związany z osłabieniem powracającego sygnału, gdy wiązka laserowa ulega odbiciu zgodnie z prawem Lamberta, przyjęto, że błąd jest odwrotnie proporcjonalny do kosinusa kąta padania wiązki: r = k 1 cos φ i + k 2. (2.83) Na rysunku 2.43 zaznaczono wartości błędu systematycznego zależnego od kąta padania wiązki φ i otrzymane eksperymentalnie, po korekcji błędu zależnego od mierzonej odległości (opis oznaczeń na rysunku). Linią ciągłą przedstawiono przebieg funkcji postaci (2.83), aproksymującej zależność r = f(φ i ). Współczynniki k 1 i k 2 wyznaczono przez dopasowanie w sensie minimalnokwadratowym i otrzymano następujący wynik: k 1 = 7,904, k 2 = 7,904. Jeżeli pomiar został wykonany przy znanym i różnym od zera kącie φ i, to skorygowana wartość zmierzonej odległości r m może być wyznaczona z uwzględnieniem odległości r m danej równaniem (2.72) oraz aproksymowanego błędu zależnego od kąta wyznaczonego ze wzoru (2.83). Na rysunku 2.43B przedstawiono błędy bezwzględne pomiaru odległości zależne od φ i w funkcji tego kąta (linie przerywane) oraz błędy bezwzględne po uwzględnieniu proponowanej korekcji (linie ciągłe). Z rysunku tego wynika, że wartość 72

bezwzględna błędu skorygowanego pomiaru w zakresie kąta padania wiązki do 60 nie przekracza 4 mm, gdy odległość wynosi od 0,5 do 4 m. A bezwzglêdny b³¹d pomiaru [mm] 0-2 -4-6 -8-10 -12 50 cm 100 cm 150 cm 200 cm 250 cm 300 cm 350 cm 400 cm B bezwzglêdny b³¹d pomiaru [mm] 4 2 0-2 -4-6 -8-10 -12 50 cm 100 cm 150 cm 200 cm 250 cm 300 cm 350 cm 400 cm -14 0 10 20 30 40 50 60 70 k¹t padania wi¹zki [ ] -14 0 10 20 30 40 50 60 k¹t padania wi¹zki [ ] Rysunek 2.43. Korekcja błędu systematycznego zależnego od kąta padania wiązki (opis w tekście) Skaner laserowy identyfikuje położenie obiektów względem robota. Pomiary dokonywane są w biegunowym układzie współrzędnych, a parametry identyfikowanego modelu są określone przez odległość r i kąt ϕ względem układu współrzędnych robota. Mierzona jest odległość r, natomiast kąt ϕ wynika z bieżącego ustawienia lustra skanera względem jego układu współrzędnych, który z kolei jest jednoznacznie usytuowany względem układu związanego z robotem (rys. 2.32). Jeżeli błędne pomiary będące wynikiem efektu śladu optycznego lub odbić zostały wyeliminowane, odczyt odległości mieści się w użytecznym zakresie, a błąd systematyczny został skorygowany opisanymi wyżej metodami, to można założyć, że niepewność pomiaru r zależy od składnika losowego. Istnieje wiele potencjalnych źródeł zakłóceń powodujących błędy przypadkowe w pomiarach dalmierzem TOF. Zakłócenia są związane głównie z wykrywaniem momentu powrotu impulsu [191] oraz precyzyjnym pomiarem bardzo krótkich przedziałów czasu [144]. W przypadku sensora Sick LMS 200 nie ma możliwości dokładnego określenia źródeł i sposobu propagacji tych zakłóceń, gdyż producent nie ujawnia szczegółowych danych dotyczących budowy dalmierza. Jednak ze względu na istnienie wielu niezależnych źródeł zakłóceń elektronicznych, a także z uwagi na wyniki przeprowadzonych eksperymentów (rys. 2.37) za model matematyczny rozrzutu wyników pomiarów odległości przyjęto rozkład normalny. Pomiar odległości jest przedstawiany jako suma prawdziwej (nieznanej) wartości r i addytywnego błędu δr, będącego zmienną losową o zerowej wartości oczekiwanej i wariancji σ 2 r: ˆr = r + δr, δr = N(0,σ r ), (2.84) gdzie ˆr jest estymatą pomiaru, a σ r odchyleniem standardowym. Ponieważ r nie jest znane, jest zastępowane dostępną oceną zmierzonej odległości. Czas potrzebny na pełen skan sensora LMS 200 jest bardzo krótki i umożliwia prowadzenie pomiarów podczas ruchu pojazdu, dlatego wykorzystanie średniej z serii pomiarów r m jest niepraktyczne wymagałoby zatrzymania robota na czas skanowania. Z uwagi na to za ocenę zmierzonej odległości przyjmuje się bieżącą wartość pomiaru po uwzględnieniu korekcji błędów systematycznych. 73

Rezygnacja z wielokrotnego powtarzania pomiarów pociąga za sobą konieczność oszacowania odchylenia standardowego odległości mierzonej w inny sposób (niepewność typu B). Ponieważ podczas eksperymentów nie stwierdzono zależności odchylenia standardowego od kąta padania wiązki, a zależność od reflektancji powierzchni obserwowanej jest nieznaczna, aproksymowano przebieg σ r = f( r m ) wielomianem drugiego rzędu postaci: σ r = a 0 + a 1 r m + a 2 r 2 m, (2.85) gdzie a i,i = 0...2, są poszukiwanymi współczynnikami. Wyznaczono dopasowanie w sensie minimum sumy kwadratów odchyłek danych eksperymentalnych od przyjętego modelu. Uzyskano współczynniki: a 2 = 2,105 10 7, a 1 = 1,415 10 3, a 0 = 4,412. Na rysunku 2.44 przedstawiono otrzymane z eksperymentów wartości σ r = f(r m ) (znak dla powierzchni białej, + dla pozostałych) oraz dopasowaną krzywą postaci (2.85) (linia ciągła). 10 9 odchylenie standardowe [mm] 8 7 6 5 4 3 2 1 0 0 1000 2000 3000 4000 5000 6000 7000 8000 odleg³oœæ zmierzona [mm] Rysunek 2.44. Aproksymacja funkcją (2.85) zależności między mierzoną odległością a odchyleniem standardowym dla skanera LMS 200 Kierując się argumentacją analogiczną do przedstawionej dla skanera KARiI, przyjęto reprezentację kąta odchylenia wiązki skanującej sensora LMS 200 w postaci sumy prawdziwej (nieznanej) wartości ϕ i addytywnego błędu δϕ, będącego zmienną losową o zerowej wartości oczekiwanej i wariancji σ 2 ϕ: ˆϕ = ϕ + δϕ, δϕ = N(0,σ ϕ ). (2.86) Skaner LMS 200 charakteryzuje się wiązką pomiarową o niewielkiej rozbieżności, a kąt ustawienia wirującego lustra określany jest za pomocą enkodera [263]. Rozdzielczość kątowa skanera w analizowanym trybie pracy ϕ = 0,5. Analogicznie jak w przypadku skanera KARiI, σ ϕ wyznaczono zakładając, że kąt 1 2 ϕ stanowi niepewność graniczną, i otrzymano wartość σ ϕ = 0,083. Za estymatę kąta ϕ przyjmuje się jego wartość nominalną wynikającą z kolejnego indeksu pomiaru w skanie. Ze względu na długość fali świetlnej skanera Sick nie istniała możliwość obserwacji śladu optycznego i eksperymentalnego określenia jego kształtu, a także kąta rozbieżności wiązki. W związku z tym przyjęto średnicę śladu podawaną przez producenta: 5 cm w odległości 74

8 m od sensora [263]. Wynikający z tych wielkości kąt rozbieżności wiązki θ LMS 0,2. Podobny kąt rozbieżności wiązki, ustalony eksperymentalnie dla skanera Sick PLS, podano w rozprawie [141]. W pracy [314] zaprezentowano wykonane w podczerwieni fotografie śladu optycznego skanera LMS 200 wskazujące, że rozkład mocy emitowanej nie ma równomiernego, kołowego kształtu, lecz ma dwa maksima lokalne, typowe dla wiązki lasera półprzewodnikowego. Jednak ze względu na małą rozbieżność wiązki oraz na to, że odbijając się od odchylonego pod kątem 45 względem płaszczyzny poziomej wirującego zwierciadła, obraca się ona wokół własnej osi w zakresie 180 (tzn. maksima lokalne przypadają w różnych miejscach śladu optycznego dla różnych kątów), zdecydowano się uznać, że cały ślad optyczny przyczynia się do powstania pomiaru z jednakowym prawdopodobieństwem. Prawdopodobieństwo istnienia przeszkody w obszarze zawartym wewnątrz wiązki ˆϕ 1 2 θ LMS, ˆϕ + 1 2 θ LMS opisane jest rozkładem jednostajnym: ( p(ˆϕ) U θ LMS 2, θ ) LMS. (2.87) 2 rozk³ad jednostajny oœ wi¹zki Y skan X rob LMS obrotowe zwierciad³o < < r d skan X skan Y rob Rysunek 2.45. Graficzna interpretacja modelu niepewności pomiaru skanerem LMS 200 Na rysunku 2.45 przedstawiono graficzną interpretację modelu niepewności pojedynczego pomiaru w skanie sensora LMS 200. 2.4.4. Podsumowanie Skanery laserowe stają się coraz popularniejszymi sensorami odległości autonomicznych robotów mobilnych i stopniowo wypierają dalmierze ultradźwiękowe. Przedstawione tutaj dwa skanery laserowe różnią się zasadą pomiaru odległości. Wynikają z tego istotne różnice parametrów oraz właściwości użytkowych. Skaner KARiI, oparty na zasadzie pomiaru przesunięcia fazowego między ciągłym sygnałem nadanym a odbitym, charakteryzuje się małą szybkością akwizycji wyników pomiarów 75

oraz silną zależnością między wynikami pomiarów odległości a intensywnością powracającego sygnału. Dominującym składnikiem niepewności pomiaru skanerem KARiI jest błąd systematyczny mierzonej odległości. Poważną wadą tego sensora jest brak kanału pomiaru intensywności powracającego sygnału, co uniemożliwia zastosowanie znanych z literatury metod kompensacji błędu systematycznego i oszacowania odchylenia standardowego pomiarów na podstawie amplitudy odbitego sygnału [3]. Ponieważ bez korekcji błędów systematycznych przydatność skanera KARiI w nawigacji robota mobilnego jest niewielka, przedstawiono procedurę korekcji opartą na zależności między błędami bezwzględnymi a odchyleniem standardowym pomiaru. Istnienie tej zależności w dalmierzu AMCW wykazano teoretycznie, a następnie na podstawie danych uzyskanych w kontrolowanym środowisku zaproponowano funkcję aproksymującą jej przebieg. Ceną korekcji błędów systematycznych jest w tym przypadku konieczność wielokrotnego powtórzenia skanowania, czyli dalsze wydłużenie czasu akwizycji pomiarów. Jednak wobec braku możliwości skanowania podczas ruchu robota uznano to rozwiązanie za lepsze niż użycie surowych pomiarów odległości. W przeciwieństwie do eksperymentalnej konstrukcji KARiI skaner Sick LMS 200 jest sensorem przemysłowym, produkowanym seryjnie i używanym często w badaniach nad robotami autonomicznymi. Wyniki pomiarów skanerem LMS 200 w o wiele mniejszym stopniu zależą od właściwości optycznych powierzchni, od których odbija się wiązka, co wynika zarówno z większej mocy nadajnika, jak i mniejszej zależności samej metody pomiaru od intensywności powracającego sygnału. Błąd systematyczny pomiarów odległości jest niewielki i jak wynika z przeprowadzonych eksperymentów, w małym stopniu zależy od reflektancji powierzchni. Z praktycznego punktu widzenia istotną zaletą skanera Sick jest duża szybkość akwizycji wyników pomiarów, pozwalająca na skanowanie podczas ruchu robota Labmate, poruszającego się z typową dla niego prędkością od 0,2 do 0,6 m s. Dla obu badanych skanerów przeanalizowano także zjawisko mixed pixels, nazywane tu efektem śladu optycznego. W przypadku sensorów laserowych jest to najpoważniejsze źródło niepewności co do przedmiotu pomiaru. Szczególnie pogarsza ono jakość danych dostarczanych przez skanery 3D [125, 314], jednak dotyczy także skanów 2D uzyskanych zarówno z sensorów klasy AMCW [4], jak i klasy TOF [226, 245, 328]. Na podstawie analizy mechanizmów powstawania tego zjawiska w obu rodzajach badanych sensorów oraz wyników badań eksperymentalnych stwierdzono, że konfiguracje środowiska oraz zakres mierzonych odległości, w którym pojawia się efekt śladu optycznego, są różne dla obu sensorów. Sprzężenie między mierzoną fazą a intensywnością sygnału w skanerze AMCW powoduje, że fantomowe pomiary mogą się pojawić nie tylko między dwoma obiektami, na które pada wiązka optyczna, lecz także przed obiektem bliższym lub za obiektem bardziej odległym. W skanerze TOF, takim jak LMS 200, nieprawidłowe pomiary odległości występują jedynie między dwoma obiektami i tylko wówczas, gdy odległość między nimi jest zbliżona do szerokości impulsu nadawanego przez dalmierz. W przypadku skanera LMS 200 nie zaobserwowano występowania efektu śladu optycznego spowodowanego padaniem wiązki na granicę powierzchni o różnej reflektancji. Wspólną dla obu klas sensorów właściwością jest wyraźnie większe odchylenie standardowe nieprawidłowych pomiarów odległości spowodowanych efektem śladu optycznego. Z literatury znane są różne podejścia do eliminacji efektu śladu optycznego. W niektórych pracach dotyczących skanerów 3D [93, 125] proponuje się zastosować filtrację medianową eliminującą odosobnione punkty w przestrzeni dwuwymiarowego obrazu odległościowego. W przypadku skanów 2D metoda ta może jednak dawać słabe rezultaty, gdyż mogą pozostawać punkty będące wynikiem podziału śladu optycznego, a co gorsze, mogą być usuwane 76

niektóre prawidłowe pomiary [4]. W pracach [314] i [329] przedstawiono metody filtracji danych 3D ze skanerów laserowych (w obu przypadkach użyto zmodyfikowanych sensorów Sick LMS 200) opracowane specjanie dla przestrzeni otwartych (ang. outdoor). W metodach tych wykorzystano charakterystyczne rozmieszczenie przestrzenne nieprawidłowych wyników pomiarów występujących w takich środowiskach. Dla skanera 2D klasy AMCW zaproponowano [3] efektywną metodę klasyfikacji pomiarów odległości na prawidłowe i będące wynikiem efektu śladu optycznego, która opiera się na wykrywaniu nieciągłości w amplitudzie odbieranego sygnału. Metoda ta wymaga jednoczesnego dostępu do pomiarów odległości i amplitudy sygnału z częstotliwością pozwalającą zebrać wystarczającą liczbę próbek w zakresie bardzo małego kąta obrotu lustra skanera, nie może więc być użyta do skanera KARiI, w którym nie ma możliwości pomiaru amplitudy, ani do skanera Sick LMS 200, opartego na innej zasadzie pomiaru niż skanery AMCW. Dla opisanego tutaj skanera AMCW opracowanego w KARiI zaproponowano metodę korekcji pomiarów wymagającą znajomości odchylenia standardowego mierzonej odległości. W proponowanym schemacie przetwarzania większość pomiarów będących wynikiem podziału śladu optycznego jest eliminowana na wstępnym etapie za pomocą równania (2.68), gdyż charakteryzują się one dużym odchyleniem standardowym. Gdy dysponuje się wartościami σ r, można także w prosty sposób wyeliminować błędne pomiary z danych skanera Sick LMS 200. Ponieważ odchylenie standardowe prawidłowych pomiarów jest niewielkie w całym użytecznym zakresie, można zastosować progowanie ze stałą wartością σ rmax. Metoda ta wymaga jednak wykonania wielu pomiarów, co znacznie wydłuża czas akwizycji danych i uniemożliwia skanowanie podczas ruchu robota. W związku z tym proponowana strategia usuwania nieprawidłowych pomiarów skanera LMS 200, przyjęta w dalszej części niniejszej pracy, opiera się na archiwizacji pomiarów odległości w postaci mapy rastrowej i wnioskowaniu o ich prawidłowości na podstawie wielu odczytów. Przedstawiona w tym podrozdziale analiza posłuży do budowy modeli matematycznych pozwalających na aktualizację mapy rastrowej oraz do wiarygodnego oszacowania niepewności pojedynczych pomiarów wykorzystywanych w budowie mapy wektorowej (parametrycznej). 2.5. Sensory wizyjne 2.5.1. Sensory wizyjne w robotach mobilnych Narząd wzroku jest podstawowym zmysłem u wielu zwierząt, szczególnie wyżej zorganizowanych i bardziej aktywnych (np. ssaków i ptaków drapieżnych). Jest to także podstawowy zmysł człowieka, dostarczający do mózgu większość informacji o otoczeniu. Także roboty mogą uzyskać najwięcej informacji o środowisku dzięki percepcji w zakresie światła widzialnego (fale elektromagnetyczne o długości od około 380 nm do 780 nm) za pomocą sensorów wizyjnych (kamer). Kamery wykorzystywane w robotach są pasywnymi sensorami matrycowymi rejestrującymi rzut perspektywiczny oświetlonego fragmentu otoczenia na zdyskretyzowaną przestrzennie płaszczyznę matrycy obrazowej. Kamery telewizyjne były używane w robotach autonomicznych w zasadzie od momentu pojawienia się tego rodzaju robotów [224]. Wczesne próby zastosowania kamer jako podstawowych sensorów wspierających nawigację robota mobilnego [212] wykazały, że widzenie maszynowe jest trudne do realizacji, głównie ze względu na złożoność metod przetwarza- 77

nia obrazu, wymagających dużej mocy obliczeniowej systemu komputerowego analizującego obraz. Klasyczny paradygmat rozwiązywania problemów widzenia maszynowego nawiązuje do warstwowego schematu przetwarzania informacji i do generowania opisu sceny (od szczegółu do ogółu). Prowadzi on do złożonego modelu otoczenia budowanego etapowo, charakteryzującego się silną redukcją ilości danych i zwiększeniem stopnia abstrakcji reprezentacji na poszczególnych etapach [197]. Wynikiem analizy obrazu jest abstrakcyjny, geometryczny i/lub semantyczny model otoczenia lub jego fragmentu. Do rekonstrukcji tego modelu niezbędne jest całkowite przeanalizowanie sceny widzianej przez kamerę. W tej sytuacji uzyskanie prostej i podstawowej dla systemu sterowania robota informacji, np. czy przed pojazdem znajduje się przeszkoda czy nie, może zająć dużo czasu. Pomimo prób zaadaptowania tego klasycznego schematu na potrzeby robotów mobilnych, np. przez wprowadzenie jawnej reprezentacji niepewności [85], oraz pomimo dość efektownych rezultatów budowy modelu otoczenia robota metodami stereowizji [330], klasyczny schemat okazał się mało przydatny, głównie ze względu na złożoność i statyczność stosowanych reprezentacji. Roboty autonomiczne wchodzą w interakcje z otoczeniem, w związku z czym muszą uwzględniać zarówno jego dynamikę (zmienność), jak i niepewność informacji dostarczanej przez sensory. W klasycznych systemach wizyjnych złożone modele otoczenia (sceny) wymagają często całkowitej przebudowy, jeżeli nowe dane z sensorów przeczą uzyskanym poprzednio. Zarysy nowego podejścia do przetwarzania obrazu, lepiej dostosowanego do specyfiki robotów autonomicznych, zostały nakreślone na podstawie obserwacji procesów percepcyjnych organizmów żywych, u których percepcja wizyjna opiera się na przypisaniu poszczególnym zachowaniom odpowiednich schematów przetwarzania [17]. W tym podejściu zakłada się specjalizację procesów przetwarzania obrazu ze względu na wspierane funkcje robota, takie jak omijanie przeszkód lub samolokalizacja. Za prekursorską w tym zakresie uważa się pracę [134], w której sformułowano koncepcję specjalizacji wizyjnych procesów percepcyjnych robota mobilnego. Na podstawie analizy cech poszczególnych zadań robota z uwzględnieniem właściwości środowiska, w którym robot ma działać, w pracy tej zaproponowano strukturę pokładowego systemu wizyjnego, wykorzystującego kilka niezależnych procesów przetwarzania obrazu, ukierunkowanych na wyodrębnienie z obrazów różnych cech otoczenia. Należy zwrócić uwagę, że przetwarzanie obrazu na potrzeby różnych funkcji robota może wymagać za każdym razem różnych algorytmów i reprezentacji [76]. Proces przetwarzania obrazu obejmuje wykrywanie, identyfikację i interpretację cech obserwowanego otoczenia na podstawie informacji zawartych w tym obrazie. W percepcji wizyjnej robotów autonomicznych proces ten jest zwykle ukierunkowany na wyodrębnienie specyficznych cech pozwalających na wykonanie określonego zadania. Cechy wykrywane na obrazie są odwzorowaniem istotnych cech obserwowanego otoczenia, takich jak nieciągłość geometrycznych, fizycznych lub fotometrycznych charakterystyk obiektów, bądź świadczą o obecności w polu widzenia robota obiektów o określonych charakterystykach [278]. Cechy istotne z punktu widzenia założonego celu percepcji są wykrywane na podstawie analizy lokalnych właściwości obrazu. Służą do tego takie operacje, jak: segmentacja, wykrywanie krawędzi, wykrywanie narożników [120]. W większości stosowanych w robotach mobilnych systemów wizyjnych wykorzystuje się kamery CCD oraz naturalne oświetlenie środowiska. Kamery rejestrują obrazy będące dwuwymiarowym rzutem rzeczywistości trójwymiarowej. Ponieważ w robotach mobilnych kamery są montowane zwykle w taki sposób, że ich osie optyczne są w przybliżeniu równole- 78

głe do podłoża, to do budowy modelu otoczenia (zarówno trójwymiarowego jak i płaskiego, 2D) konieczne jest określenie na podstawie danych z kamery (lub kamer) położenia obserwowanego punktu w przestrzeni lub na płaszczyźnie, po której porusza się robot (dla modelu 2D). Poważnym ograniczeniem w stosowaniu pasywnych systemów wizyjnych w robotach autonomicznych jest duża wrażliwość metod analizy obrazu z kamer na takie czynniki zewnętrzne jak zmiany oświetlenia, cienie, tekstura powierzchni [86]. Czynniki te nie tylko wprowadzają do obrazu zakłócenia, ale są także przyczyną niepewności co do przedmiotu pomiaru. Niepewność ta przejawia się występowaniem w obrazie cech, które nie są odwzorowaniem nieciągłości geometrycznych, fizycznych lub fotometrycznych charakterystyk obserwowanych obiektów rzeczywistych. Niepewność co do przedmiotu pomiaru w obrazie powoduje trudności w realizacji niektórych operacji, takich jak wykrywanie krawędzi czy dopasowanie cech w stereowizji [148]. Wiele robotów mobilnych wykorzystuje sztuczne znaczniki (landmarki) rozmieszczone w otoczeniu. Zastosowanie sztucznych znaczników, będących obiektami o znanych i niezmiennych atrybutach (kształt, wymiary, barwa itp.), istotnie poprawia możliwości percepcyjne pokładowych systemów wizyjnych. Część z problemów związanych z niepewnością co do przedmiotu pomiaru podczas obserwacji naturalnych cech otoczenia za pomocą kamer można wyeliminować przez odpowiednie zaprojektowanie sztucznego znacznika [153]. Inna możliwość zastosowania sensorów wizyjnych pojawia się w przypadku robotów mobilnych realizujących swoje zadania na ograniczonym obszarze (np. hala produkcyjna, magazyn), na którym można je obserwować z zewnątrz, np. za pomocą kamer związanych z otoczeniem. Rozwiązanie takie jest szczególnie efektywne w odniesieniu do systemów wielorobotowych, wówczas pewna liczba sensorów stacjonarnych (w tym przypadku kamer) może dostarczać dane nawigacyjne większej liczbie robotów. Wykorzystanie sensorów związanych z otoczeniem i będących częścią zewnętrznej infrastruktury nawigacyjnej może zmniejszyć koszty użycia zespołu robotów mobilnych [136]. Rozwiązania takie były też proponowane jako metoda samolokalizacji dla przemysłowych AGV [171]. Kamery są obecnie często wykorzystywane w robotach mobilnych, jednak kompleksowa analiza tego zagadnienia wykracza poza ramy niniejszej pracy. Zainteresowany czytelnik znajdzie aktualny przegląd zastosowań widzenia maszynowego w nawigacji robotów w pracy [86]. Typowe konfiguracje sensorów wizyjnych stosowanych w robotach mobilnych są natomiast przedstawione w artykule [301]. Powyższa krótka analiza wykorzystania metod widzenia maszynowego w robotach autonomicznych służy do uzasadnienia koncepcji użycia w systemie percepcyjnym robota mobilnego niezależnych i stosunkowo prostych wyspecjalizowanych modułów wizyjnych realizujących zadania pozycjonowania. W kolejnych częściach niniejszego podrozdziału przedstawiono dwa takie moduły: system zewnętrznych kamer monitorujących i system pozycjonowania robota na podstawie obserwacji sztucznych znaczników nawigacyjnych. Analizuje się tam wykorzystanie kamer pokładowych robotów mobilnych oraz kamer montowanych do stropu pomieszczenia (nazywanych monitorującymi) jako źródeł danych nawigacyjnych komplementarnych lub konkurencyjnych wobec siebie i innych sensorów robota. Zastosowano sztuczne znaczniki montowane do obiektów w otoczeniu robotów oraz aktywne znaczniki diodowe umieszczone na samych robotach. Dzięki temu możliwe było użycie stosunkowo prostych metod przetwarzania obrazu, pozwalających jednak precyzyjnie wyznaczyć położenie i orientację robotów mobilnych. Zastosowano wieloagentową architekturę systemu, w której kamery monitorujące są niezależnymi agentami percepcyjnymi (por. rozdz. 5). Zadaniem agentów percepcyjnych jest określanie położenia 79

i orientacji agentów robotów. Opisane rozwiązania są wykorzystywane w nawigacji robotów mobilnych będących na wyposażeniu Laboratorium Robotów Mobilnych IAiII. Uzupełniają one ich systemy percepcyjne oparte na sensorach odległości. Przedstawiając dwa różne systemy pozycjonowania, zwrócono szczególną uwagę na analizę niepewności pozycji robota wynikającej z użytych metod przetwarzania obrazu. 2.5.2. Sposób analizy i modelowania niepewności sensorów wizyjnych Istnieje wiele czynników mogących wpłynąć na niepewność informacji uzyskiwanych z pasywnych systemów wizyjnych wykorzystywanych do nawigacji robota mobilnego. Źródłami niepewności są zniekształcenia wprowadzane przez układ optyczny kamery, szum generowany przez układ elektroniczny kamery (matrycę CCD lub CMOS) oraz zakłócenia powstające w torze akwizycji obrazu (interfejsach, przewodach, złączach). Niektóre z powyższych czynników wprowadzają błędy o charakterze systematycznym, np. nieliniowe zniekształcenia obrazu wprowadzane przez obiektyw kamery. Tego rodzaju błędy powinny być usunięte przez odpowiednią kalibrację kamery. Kalibracja ma na celu precyzyjne ustalenie relacji między przestrzennym położeniem punktów obserwowanej sceny a położeniem odpowiadających im pikseli na matrycy obrazu. W zakres pełnej kalibracji wchodzi ustalenie parametrów zewnętrznych (ang. extrinsic) i wewnętrznych (ang. intrinsic) kamery oraz wyznaczenie współczynników korygujących błędy wprowadzane przez niedoskonałość układu optycznego. Kalibracja parametrów zewnętrznych pozwala ustalić położenie i orientację płaszczyzny obrazu (matrycy) względem określonego układu odniesienia (np. układu robota lub układu globalnego związanego z otoczeniem), natomiast parametry wewnętrzne określają związek między geometrycznymi współrzędnymi punktów w płaszczyźnie obrazu a dyskretnymi współrzędnymi pikseli w matrycy obrazowej [137]. Istnieje wiele dobrze znanych metod kalibracji kamer [127, 311]. Charakterystyki niektórych źródeł niepewności są związane ze sposobem wykorzystania kamery, np. niektóre rodzaje zakłóceń (szumów) w matrycach CCD ujawniają się w warunkach słabego oświetlenia lub długotrwałej ekspozycji. Jakkolwiek zakłócenia takie można modelować, to ustalenie ich wielkości w przypadku kamery umieszczonej na ruchomym robocie w zmiennym środowisku jest trudne, gdyż raz zmierzone charakterystyki mogą ulec szybkim i nieprzewidywalnym zmianom. Wobec takich problemów z wiarygodnym ustaleniem wpływu zakłóceń z poszczególnych źródeł, przede wszystkim związanych z kamerą jako urządzeniem elektronicznym, przyjęto następujący sposób analizy niepewności przedstawionych w kolejnych podrozdziałach wizyjnych metod percepcji: Dla przemysłowych kamer CCD przyjęto prosty model geometryczny typu pin-hole o znanych parametrach. W dalszych rozważaniach założono, że parametry te nie wprowadzają niepewności. Pominięto źródła takich zakłóceń, które nie zależą lub zależą w niewielkim stopniu od usytuowania robota względem obserwowanej cechy środowiska (np. nie zależą od odległości od danego obiektu, od kąta widzenia landmarku). Do pominiętych zakłóceń należą szumy pochodzenia elektronicznego, pogorszenie ostrości i kontrastu obrazu wraz ze zmianą odległości obserwacji. Zidentyfikowano wyjściowe źródła niepewności, charakterystyczne dla użytych sensorów wizyjnych i sposobu ich wykorzystania (kamery przemysłowej związanej z oto- 80

czeniem i kamery pokładowej), a następnie określono propagację niepewności na kolejnych etapach procesu pomiarowego, aż do uzyskania opisu niepewności końcowego parametru użytecznego dla systemu nawigacji robota. Ponieważ założono rozkład normalny zmiennych losowych opisujących niepewność wyjściową i parametry poszczególnych etapów przetwarzania informacji, wykorzystano propagację wariancji [124]. Zastosowano rozkład w szereg Taylora nieliniowych równań opisujących związki pomiędzy poszczególnymi zmiennymi, definiując jakobiany odpowiednich przekształceń. Postępowanie takie jest standardowo przyjmowane w robotyce [289], a w komputerowym przetwarzaniu obrazów wykorzystano je między innymi do modelowania niepewności systemu stereowizyjnego [208]. Daje ono dobre wyniki, jeśli błędy pomiarów są małe, nie mają charakteru systematycznego (dzięki prawidłowej kalibracji), a ich źródła są niezależne. W każdym z opisywanych systemów percepcji wizyjnej proces analizy niepewności informacji doprowadzono do postaci zamkniętych formuł opisujących zależność niepewności estymaty pozycji robota dostarczanej przez system wizyjny od bieżącej konfiguracji tego robota względem istotnych elementów środowiska (kamery monitorującej, landmarku). Przez konfigurację jest tu rozumiane położenie i orientacja robota względem lokalnego układu współrzędnych współpracującego sensora lub sztucznego znacznika. Na podstawie macierzy kowariancji uzyskanej w wyniku propagacji niepewności można wyznaczyć za pomocą wzorów (2.22) elipsę niepewności, wewnątrz której z określonym prawdopodobieństwem P (2.23) znajduje się robot. Elipsa ta jest wygodną reprezentacją jakości estymaty położenia robota mobilnego. Możliwe jest też sporządzenie mapy przyporządkowującej poszczególnym konfiguracjom robota skalarną miarę niepewności położenia. Miarą taką jest pole powierzchni elipsy niepewności. Im większa jest powierzchnia elipsy dla danego prawdopodobieństwa P, tym mniej dokładnie jest zlokalizowany robot. Jeżeli wiadomo, że środek robota, który może się obracać w miejscu (np. ma układ jezdny typu różnicowego, tak jak Labmate), znajduje się z określonym prawdopodobieństwem we wnętrzu danej elipsy, to można stwierdzić na podstawie bieżącej estymaty położenia, czy robot może wykonać założony ruch bez kolizji z przeszkodami [211]. 2.5.3. Kamery monitorujace Kamera monitorujaca jako sensor pozycjonujacy robota Zadaniem kamer monitorujących jest pozycjonowanie robotów jako alternatywa dla innych metod. Dwie kamery są umieszczone pod stropem pomieszczenia. Wykorzystano dostępne w Laboratorium Robotów Mobilnych IAiII czarno-białe kamery przemysłowe z szerokokątnymi obiektywami o ogniskowej f = 2,8 mm. Kamery monitorujące pomieszczenie wraz z kartami akwizycji obrazu i komputerami stacjonarnymi stanowią bazę sprzętową agentów percepcyjnych. Możliwość pozycjonowania robotów mobilnych za pomocą agentów związanych z kamerami monitorującymi zależy głównie od położenia robota względem osi optycznej obiektywu kamery danego agenta oraz od aktualnego oświetlenia sceny. W systemie pozycjonowania za pomocą kamer monitorujących należy zwrócić uwagę na zapewnienie dużego pola widzenia kamer bez nadmiernego zwiększania ich liczby oraz zapewnienie dokładnego i niezawodnego rozpoznawania robotów o różnych kształtach w zmiennych warunkach oświetlenia [37]. 81

Duże pole widzenia kamer zapewniono dzięki zastosowaniu obiektywów szerokokątnych o bardzo krótkiej ogniskowej (tzw. rybie oko). Obiektywy o krótkich ogniskowych wprowadzają jednak istotne zniekształcenia obrazu obserwowanej sceny, polegające na przesunięciu pikseli obrazu względem ich oryginalnych pozycji, wynikających z rzutowania geometrycznego [36, 37]. Analiza i rozpoznawanie takiego obrazu, a w szczególności wykorzystanie go do niezbędnego w pozycjonowaniu robotów pomiaru wielkości geometrycznych wymaga korekcji zniekształceń. W celu jednoznacznej identyfikacji robotów na scenie posłużono się systemem z aktywnymi znacznikami w postaci czterech diod LED zamontowanych do obudowy robota. Sekwencja zapalania/gaszenia diod jest sterowana z komputera pokładowego, co pozwala uzyskać pary obrazów sceny z diodami zapalonymi i zgaszonymi, a następnie wyznaczyć z nich binarny obraz różnicowy. Obraz taki pozwala zidentyfikować robota bardziej jednoznacznie niż np. segmentacja obrazu szarego, która jest trudna w przypadku obrazów robotów o skomplikowanych kształtach i wymaga silnych założeń (np. co do rodzaju tła [154]). Na obrazie różnicowym pojawiają się segmenty o wielkości kilku pikseli odpowiadające włączonym diodom. Na idealnym obrazie są widoczne cztery grupy pikseli. W praktyce zawsze pojawiają się zakłócenia pochodzące np. od ruchomych obiektów oraz losowych zmian natężenia naturalnego oświetlenia. Większość tych zakłóceń może być wyeliminowana przez zastosowanie kryterium maksymalnej i minimalnej wielkości, na podstawie którego klasyfikuje się daną grupę jako obraz diody. Spośród pozostałych grup pikseli właściwe wybierane są na podstawie znajomości wymiarów robota i układu diod. Procedura wyznaczania położenia i orientacji robota na podstawie obrazów z kamery monitorującej składa się z następujących etapów: Pobranie dwóch obrazów z kamery: z włączonymi (rys. 2.46A) i wyłączonymi diodami robota. Eliminacja nierównomiernego oświetlenia sceny. Wyznaczenie obrazu różnicowego i jego progowanie (rys. 2.46B). Etykietowanie obiektów i usunięcie obiektów niemieszczących się w przyjętym przedziale wielkości (rys. 2.46C). Wyznaczenie położenia obiektów (rys. 2.46D). Korekcja zniekształceń wprowadzanych przez obiektyw rybie oko (rys. 2.46E). Znajdowanie na obrazie punktów odpowiadających wskaźnikom diodowym robota (rys. 2.46F). Określenie położenia i orientacji robota. Konwersja współrzędnych lokalnych (obrazu) na współrzędne globalne. Wszystkie operacje w procedurze rozpoznawania robotów prowadzone są na wycinku obrazu określonym jako okno poszukiwań [37]. Położenie okna jest określane na podstawie znajomości prawdopodobnej lokalizacji robota, którą to informację sam robot przesyła do agenta kamery monitorującej. Do eliminacji nierównomiernego oświetlenia sceny zastosowano filtr homomorficzny [156], dzięki któremu stłumiono składową oświetleniową (luminancyjną) obrazu. Składowa 82

A B C D E F Rysunek 2.46. Główne etapy przetwarzania obrazu z kamery monitorującej (opis w tekście) luminancyjna zmienia się zazwyczaj wolno, natomiast składowa refleksyjna szybko. Wyodrębniając składową refleksyjną, otrzymuje się obraz o wyeksponowanych odblaskach i przytłumionym tle. Separacja nie jest doskonała, lecz późniejszy obraz różnicowy dwóch klatek ma znacznie mniej zakłóceń niż bez filtracji. Binarny obraz różnicowy I B two- rzony jest na podstawie pary obrazów z włączonymi i wyłączonymi diodami. Grupa połączonych pikseli na obrazie binarnym stanowi potencjalną reprezentację poszukiwanego obiektu (diody). Za pomocą algorytmu etykietowania znajduje się wszystkie połączone punkty obiektów na obrazie i przypisuje im unikatową wartość (etykietę) [309]. Usuwane są grupy punktów tworzące obiekty zbyt duże i zbyt małe, by być obrazami diod robota. Określanie środków obiektów znalezionych na obrazach binarnych odpowiada wyznaczeniu środka masy [284]. A Rysunek 2.47. Geometria obrazu dla normalnej (A) i krótkiej (B) ogniskowej Przed wykorzystaniem obrazu do określenia położenia robotów należy dokonać kalibracji parametrów wewnętrznych kamery [127], a ze względu na zastosowany obiektyw szerokokątny należy także skompensować zniekształcenia przez użycie prostującego przekształcenia geometrycznego. Polega ono na transformacji współrzędnych obrazowych w taki sposób, aby obraz wynikowy odwzorowywał obserwowane środowisko tak, jakby było ono obserwowane z bardzo dużej odległości z użyciem obiektywu o długiej ogniskowej, który nie wprowadza zniekształceń (rys. 2.47). Spośród kilku znanych metod prostowania zniekształconego obrazu [259] wybrano metodę transformaty sferycznej zaproponowaną w pracy [36]. W przypadku B 83

pozycjonowania robota za pomocą kamery monitorującej transformata prostująca nie musi być stosowana do wszystkich pikseli obrazu, lecz jedynie do wyznaczonych środków obiektów. Przyśpiesza to przetwarzanie obrazu. Gdy znane są środki wszystkich obiektów, znalezionych dzięki wcześniejszym krokom detekcji diod, należy wyodrębnić tylko te, których wzajemne położenie na obrazie odwzorowuje kształt i wielkość układu wskaźników diodowych robota. Dla każdego środka szuka się punktów oddalonych o odległość d 1 (większą odległość pomiędzy diodami) oraz punktów oddalonych o odległość d 2. Sprawdza się kąty pomiędzy poszczególnymi odcinkami tworzącymi trójkąt, by stwierdzić, czy jest to trójkąt prostokątny. Jeżeli warunek odległości i kąta jest spełniony, to dany trójkąt będzie zaakceptowany. Następnie sprawdza się, czy wyznaczone trójkąty stanowią triangulację prostokąta [242]. W zależności od liczby dostrzeżonych diod (jedna dioda może być przesłonięta przez elementy obudowy robota) można otrzymać jeden trójkąt dla trzech diod lub cztery trójkąty dla czterech diod. Mając dane współrzędne obrazowe punktów odpowiadających położeniu wskaźników diodowych na obrazie, należy określić położenie i orientację robota we współrzędnych obrazowych U R = [u R v R ϑ R ] T. Wyznacza się środek układu diod będący początkiem układu współrzędnych robota. Określa się położenia środków przeciwprostokątnych trójkątów prostokątnych wykrytych w poprzednim etapie. Pozwala to wyznaczyć położenie robota, gdy widoczne są tylko trzy diody. Jeżeli znaleziono więcej niż jeden trójkąt, to wyniki się uśrednia. Dla jednego trójkąta u R = u A + u B 2, v R = v A + v B, (2.88) 2 gdzie (u A,v A ) i (u B,v B ) to współrzędne najdłuższego boku trójkąta. Do wyznaczenia orientacji tworzonego przez diody prostokąta potrzebne są współrzędne tylko dwóch punktów oraz informacja, że tworzą one dłuższy bok prostokąta, uzyskiwana przez porównanie odległości między wykrytymi diodami ze znanymi wymiarami układu znaczników: ( ) vb v A ϑ R = arctg. (2.89) u B u A Jeżeli u A = u B, to orientacja robota wynosi 90. Jeżeli widoczne były cztery diody, to można określić orientację na podstawie pozostałej pary diod, a wyniki uśrednić. Gdy jest wyznaczone położenie środka robota we współrzędnych obrazowych, można określić jego położenie w układzie kamery z wykorzystaniem proporcji pomiędzy współrzędnymi obrazowymi i rzeczywistymi wymiarami na scenie: x R = (u R w 2 ) w, y w R = (v R h 2 ) h h, θ R = ϑ R, (2.90) gdzie w i h są wymiarami matrycy CCD, a w i h wymiarami pola widzenia kamery. Orientacja robota w układzie kamery jest taka sama jak we współrzędnych obrazowych. Współrzędne robota x R w układzie globalnym wyznacza się na podstawie następujących zależności: x R = x k + x R cos θ k y R sin θ k, (2.91) y R = y k + x R sinθ k + y R cos θ k, (2.92) θ R = θ k + θ R, (2.93) 84

gdzie x K = [x k y k θ k ] T i jest wektorem położenia i orientacji kamery w globalnym układzie współrzędnych. Analiza niepewności Ponieważ zadaniem kamer monitorujących jest pozycjonowanie robotów mobilnych znajdujących się w ich polu widzenia, istotna jest znajomość niepewności, z jaką agent percepcyjny związany z kamerą monitorującą może wyznaczyć położenie i orientację robota. Niepewność ta jest wyrażona macierzą kowariancji C R (2.20) wektora położenia i orientacji robota x R. W przedstawionej niżej procedurze wyznaczania macierzy C R założono, że położenie punktu sceny, odwzorowanego w środku obrazu, można określić bezbłędnie i że na błąd pomiaru położenia pojedynczego piksela (na matrycy CCD) wpływają jedynie niedokładność procedury korekcji zniekształcenia typu rybie oko oraz błąd wynikający z odchylenia od pionu osi optycznej kamery monitorującej. Pozostałe źródła błędów nie są uwzględniane, gdyż ich wpływ na dokładność określenia pozycji robota jest niewielki, jeśli założy się prawidłową kalibrację kamery [37]. Proces lokalizacji robota wymaga znalezienia współrzędnych co najmniej trzech punktów sztywno związanych z pojazdem, w rozpatrywanym przypadku aktywnych znaczników LED. Przyjmuje się, że ustawienie diod względem układu współrzędnych robota jest dokładnie znane (nie jest obarczone niepewnością). Danymi wejściowymi są wykryte położenia diod robota na matrycy CCD kamery monitorującej U D = [u d v d ] T oraz ich macierze kowariancji: [ ] σ 2 C D = u 0 0 σv 2. (2.94) Ponieważ położenie środka robota określa się za pomocą wzoru (2.88), bierze się pod uwagę współrzędne dwóch diod U DA i U DB, których macierze kowariancji C DA, C DB są wyznaczane na podstawie błędów lokalizacji pojedynczego piksela, zależnych od położenia danej diody na matrycy CCD: C D (u,v) = C m (u,v) + C f (u,v), (2.95) gdzie macierz C f odzwierciedla błąd związany z transformatą prostującą, a C m błąd związany z niedokładnie pionową osią optyczną kamery. Odpowiednie elementy tych macierzy wyznacza się na podstawie tablic błędów zawierających wartości odchylenia standardowego σ u i σ v (w pikselach) dla wszystkich pikseli matrycy CCD uzyskane eksperymentalnie [264]. Do dalszych obliczeń przyjęto wartości bezwzględne najgorszego przypadku błędów rzeczywistych. Procedura wyznaczania błędów jest przedstawiona szczegółowo w pracy [36]. Błąd wynikający z niedoskonałości korekcji zniekształceń geometrycznych został zobrazowany na rys. 2.48A i B, a na rys. 2.48C i D przedstawiono błędy spowodowane odchyleniem osi kamery od pionu o 1. Aby wyznaczyć macierz kowariancji C UR dla położenia i orientacji robota U R we współrzędnych obrazowych, wykorzystuje się zależność: C UR = J UA C DA J T U A + J UB C DB J T U B, (2.96) gdzie J U to jakobian transformacji (2.88) i (2.89) względem współrzędnych u i v. Ponieważ dwa wskaźniki diodowe umożliwiają określenie położenia oraz orientacji robota za pomocą 85

0 A 12 B 12 u [piksele] 10 8 6 4 2 v [piksele] 10 8 6 4 2 0 500 400 300 200 u [piksele] 100 0 100 200 300 600 500 400 v [piksele] 700 0 500 400 300 u [piksele] 200 100 0 0 100 200 300 600 500 400 v [piksele] 700 25 C D 18 20 16 u [piksele] 15 10 5 v [piksele] 14 12 10 8 6 4 0 500 400 300 200 u [piksele] 100 0 0 100 200 300 600 500 400 v [piksele] 700 2 0 500 400 300 200 u [piksele] 100 0 0 100 200 300 600 500 400 v [piksele] 700 Rysunek 2.48. Rozkład błędów położenia pikseli na obrazie w osi u (z lewej) i w osi v (z prawej) trzech funkcji (dwa równania opisują położenie, a jedno orientację), jakobian ma następującą postać: J U = u R u v R u ϑ R u u R v v R v ϑ R v = 1 2 0 0 1 2 u A u B (u A u B) 2 +(v A v B) 2 v B v A (u A u B) 2 +(v A v B) 2. (2.97) Dotychczasowe obliczenia dotyczyły współrzędnych obrazowych. Macierz kowariancji C UR (2.96) należy odpowiednio przekształcić, by otrzymać macierz określającą niepewność położenia i orientacji w układzie kamery. Ponieważ przekształcenie (2.90) jest liniowe, uzyskuje się zależność: σ 2 σ C x y x σ θ x w 0 0 R = σ x y σ2 σ w σ y θ y = h 0 0 u 2 σ vu σ θu σ σ x θ σ h uv σv 2 σ θv. (2.98) y θ σ2 0 0 1 σ θ uϑ σ vϑ σϑ 2 Niepewność położenia i orientacji C R w układzie globalnym wyznacza się z zależności: C R = J K C RJ T K, (2.99) gdzie J K jest jakobianem transformacji (2.91 2.93) względem wektora X R : 86

J K = x R x R y R x R θ R x R Przyjmuje się, że położenie i orientacja kamery w globalnym układzie współrzędnych są dokładnie znane, a mogąca wynikać stąd niepewność nie jest brana pod uwagę. Wykorzystanie kilku niezależnych pomiarów może prowadzić do zmniejszenia niepewności pozycji robota w porównaniu z niepewnością oszacowaną na podstawie błędu lokalizacji pojedynczych pikseli na matrycy CCD. Jeżeli zaobserwowane zostały cztery wskaźniki diodowe i na ich podstawie można określić dwa różne trójkąty prostokątne, to również x R x R y R θ R y R y R y R θ R θ R θ R y R θ R = pole elipsy niepewnoœci po³o enia [mm 2 ] x 10 4 4 3.5 3 2.5 2 1.5 1 0.5 0 4000 2000 x [mm] cos θ k sin θ k 0 sin θ k cos θ k 0. (2.100) 0 0 1 0-2000 -4000-3000 Y X -2000-1000 1000 0 y [mm] Rysunek 2.49. Niepewność położenia jako funkcja ustawienia robota względem osi kamery można wyznaczyć dwa wektory położenia i orientacji robota x R1 i x R2 oraz dwie macierze kowariancji C R1 i C R2. Za pomocą prostego, statycznego filtru Kalmana można połączyć obie estymaty pozycji robota w jedną, uwzględniając niepewność i otrzymując estymatę dokładniejszą: K = C R1 (C R1 + C R2 ) 1, (2.101) ˆx R = x R1 K(x R1 x R2 ), (2.102) C R = (I K)C R1, (2.103) gdzie ˆx R jest pozycją po fuzji danych, C R jej macierzą kowariancji, a K wzmocnieniem filtru. Na rysunku 2.49 przedstawiono mapę niepewności estymaty położenia robota [ˆx R ŷ R ] T w zależności od jego ustawienia względem kamery monitorującej. Przyjętą miarą niepewności położenia jest pole elipsy niepewności wyznaczonej na podstawie macierzy C R (2.99). Mapa niepewności w postaci trójwymiarowego wykresu służy jedynie do wizualizacji otrzymanych wyników, natomiast w procedurach planowania akcji pozycjonujących robota mobilnego, przedstawionych w rozdziale 6, wykorzystuje się wyprowadzone powyżej zależności analityczne. 2.5.4. Pozycjonowanie za pomoca kamery pokładowej i landmarków Kamera pokładowa jako sensor służacy do pozycjonowania robota Kamera pokładowa sztywno związana z platformą pojazdu jest wykorzystywana do wykrywania sztucznych znaczników (landmarków) w otoczeniu robota. Ponieważ kolor jest istotnym czynnikiem w wykrywaniu proponowanych landmarków [153], robot został wyposa- 2000 3000 87

żony w kolorową kamerę CCD. Dla omawianego zadania za optymalny uznano obiektyw o stałej ogniskowej f = 8 mm. Zapewnia on stosunkowo szerokie pole widzenia, nie zniekształcając istotnie geometrii obrazu. Pozycjonowanie oparto na rozmieszczo- landmark X Y k¹t (niepewnoœæ) odleg³oœæ (niepewnoœæ) l robot mobilny z kamer¹ pok³adow¹ Y k¹t (niepewnoœæ) Rysunek 2.50. Pozycjonowanie za pomocą landmarku X nych w otoczeniu robota sztucznych znacznikach nawigacyjnych o rozmiarze arkusza formatu A4. Rozpoznanie przez system wizyjny landmarku w otoczeniu robota oraz wyznaczenie na tej podstawie dokładnej pozycji robota wymaga zastosowania złożonej procedury przetwarzania obrazu, opisanej szczegółowo w pracy [38]. Landmarki są opatrzone unikatowymi kodami odczytywanymi przez system wizyjny. Pozwala to na umieszczenie w otoczeniu robota do 256 rozróżnialnych znaczników. Położenie poszczególnych landmarków w otoczeniu oraz przypisane im kody są znane a priori. Wykorzystano model kamery typu pinhole, która jest wewnętrznie skalibrowana (długość jej ogniskowej jest dokładnie znana) [311]. Dla uproszczenia rozważań przyjęto, że początek układu współrzędnych kamery pokładowej pokrywa się ze środkiem robota (rys. 2.50). Proces rozpoznawania landmarków składa się z trzech podstawowych etapów: detekcji obszarów zainteresowania (potencjalnych landmarków) na obrazie, rozpoznania landmarków wewnątrz poszczególnych obszarów, ustalenia współrzędnych obrazowych charakterystycznych punktów landmarku. Dane otrzymane z obrazu landmarku są wykorzystywane do obliczenia wektora L = [l ϕ 1 ϕ 2 ] T, określającego pozycję kamery (a więc także robota) względem obserwowanego landmarku: l = v l ( ) 2 λ 2 + x1y 2+x 2y 1 y 1+y 2 2y 1y 2 y 1+y 2, ϕ 1 = arctg ( y 1 y 2 ϕ 2 = arctg λ x 1 y 2 + x 2 y 1 ( x1y 2+x 2y 1 y 1+y 2 λ ), (2.104) ), (2.105) gdzie v l jest połową wysokości landmarku, λ długością ogniskowej kamery, a współrzędne poziome x 1 i x 2 lewej i prawej pionowej krawędzi landmarku oraz połowy długości y 1 i y 2 tych krawędzi są wyznaczane z obrazu landmarku [38]. Ponieważ położenie i orientacja landmarku o określonym kodzie w globalnym układzie współrzędnych są znane, wyznaczenie położenia i orientacji robota w układzie globalnym jest proste: x R = x li +l cos(θ li +ϕ 2 ϕ 1 ), y R = y li +l sin(θ li +ϕ 1 ϕ 2 ), θ R = π+θ li ϕ 2, (2.106) 88

gdzie x Li = [x li y li θ li ] T są współrzędnymi i-tego landmarku w układzie globalnym. Jeżeli landmark nie jest umieszczony na obiekcie stacjonarnym, lecz przymocowany do korpusu innego robota mobilnego, to robot ten musi dostarczyć estymatę swojego położenia i orientacji w układzie globalnym, która będzie uwzględniona w równaniach (2.106). Analiza niepewności Poniżej przedstawiono analizę niepewności informacji dostarczanych przez kamerę pokładową traktowaną jako wyspecjalizowany sensor służący do pozycjonowania robota i współpracujący ze sztucznymi znacznikami w jego otoczeniu. Wiele czynników może wpłynąć na niepewność estymaty położenia i orientacji robota mobilnego wyznaczonej na podstawie obserwacji sztucznego landmarku kamerą pokładową. W przedstawionej analizie pominięto te, które nie zależą od pozycji robota względem obserwowanego landmarku. Parametry landmarku oraz jego położenie i orientację w układzie globalnym uznano za dokładnie znane nie wprowadzają one dodatkowej niepewności. Zakłada się także, że środek optyczny kamery związanej z robotem znajduje się w początku jego układu współrzędnych, a oś optyczna kamery pokrywa się z osią 0X układu robota. Rzeczywista dokładność pozycjonowania robota może być oceniona jedynie eksperymentalnie [38]. Proponowana tu procedura oszacowania niepewności ma na celu umożliwienie stwierdzenia a priori, jak się będzie kształtować niepewność pozycjonowania opartego na obserwacji landmarków, których położenie i orientacja (względem globalnego układu odniesienia) są znane. Z tego powodu uwzględniono jedynie niepewność spowodowaną wyznaczeniem współrzędnych obrazowych charakterystycznych punktów (narożników) ze skończoną dokładnością. Dokładność ta silnie zależy od odległości robota od obserwowanego landmarku oraz kąta, pod jakim jest on obserwowany. Odległości i kąty pomiędzy kamerą pokładową robota a obserwowanym przez nią sztucznym landmarkiem są wyznaczane na podstawie wymiarów obrazu landmarku uzyskanego na matrycy CCD kamery oraz znajomości rzeczywistych wymiarów landmarku. Kamera traktowana jest jako narzędzie do pomiaru wielkości obrazu. Rozdzielczość tego narzędzia jest ograniczona wielkością pojedynczego piksela matrycy CCD. Z tego powodu, chociaż położenie charakterystycznych punktów landmarku [x 1 y 1 ] i [x 2 y 2 ] jest wyznaczane z dokładnością subpikselową [38], odchylenie standardowe tak rozumianego pomiaru jest nie mniejsze niż jeden piksel [127, 208]. Jest to początkowa niepewność, wprowadzana przez skończoną rozdzielczość kamery, która jest następnie przenoszona (propagowana) na niepewność parametrów wektora L, a dalej na niepewność pozycji robota x R w globalnym układzie współrzędnych. Błędy wyznaczenia współrzędnych P = [x 1 y 1 x 2 y 2 ] T na obrazie są reprezentowane przez odchylenia standardowe [σ x1 σ y1 σ x2 σ y2 ] T. Stąd początkowa niepewność opisana jest macierzą 4 4: C P = σ 2 x1 0 0 0 0 σ 2 y1 0 0 0 0 σ 2 x2 0 0 0 0 σ 2 y2. (2.107) Odchylenie standardowe σ y w osi y określone jest przez pionowy błąd dyskretyzacji obrazu: σ y = h R h, (2.108) 89

gdzie R h jest pionową rozdzielczością matrycy CCD, a h jej wysokością (R h = 480, h = 3,6 mm). Podobnie, odchylenie standardowe σ x w osi x zdefiniowane jest jako σ x = w R w, (2.109) gdzie R w jest poziomą rozdzielczością matrycy CCD, a w jej szerokością (R w = 640, w = 4,8 mm). Niepewność wektora L opisana jest macierzą kowariancji C L : σl 2 σ ϕ1l σ ϕ2l C L = σ lϕ1 σϕ 2 1 σ ϕ2ϕ 1, (2.110) σ lϕ2 σ ϕ1ϕ 2 σϕ 2 2 którą oblicza się przez propagację niepewności z macierzy kowariancji C P wektora P. Parametry tego wektora służą jako zmienne wejściowe procedury wyznaczania wektora L. Transformacja P na L zdefiniowana jest równaniami (2.104) i (2.105) zawierającymi tylko parametry P oraz stałe opisujące kamerę i landmark. Ponieważ transformacja P na L jest nieliniowa, macierz kowariancji C L jest obliczana jako aproksymacja pierwszego rzędu: C L = J P C P J T P, (2.111) gdzie J P jest jakobianem transformacji (2.104) i (2.105) względem wektora P: J P = l x 1 l y 1 l x 2 l y 2 ϕ 1 x 1 ϕ 1 y 1 ϕ 1 x 2 ϕ 1 y 2 ϕ 2 x 1 ϕ 2 y 1 ϕ 2 x 2 ϕ 2 y 2. (2.112) Analityczna postać składników jakobianu (2.112) jest podana w pracy [283]. Na rysunku 2.51 przedstawiono niepewność, z jaką wyznaczana jest odległość l w zależności od ustawienia robota względem obserwowanego landmarku. Pozycja kamery (a więc robota) w globalnym układzie współrzędnych wyrażona jest wektorem parametrów x R = [x R y R θ R ] T i określona równaniami (2.106). Niepewność położenia i orientacji robota w układzie globalnym opisana jest macierzą kowariancji C R. Niepewność ta jest rezultatem propagacji wyznaczonej wcześniej niepewności parametrów wektora L = [l ϕ 1 ϕ 2 ] T, wyrażonej macierzą C L. Ponieważ wyrażona równaniem (2.106) zależność między wektorami L i x R jest nieliniowa, macierz kowariancji C R wyznacza się ze wzoru: C R = J L C L J T L, (2.113) gdzie J L jest jakobianem transformacji (2.106) względem wektora L: J L = x R l y R l θ R l x R ϕ 1 y R ϕ 1 θ R ϕ 1 x R ϕ 2 y R ϕ 2 θ R ϕ 2, (2.114) cos(θ l ϕ 1 + ϕ 2 ) l sin(θ l + ϕ 1 ϕ 2 ) l sin(θ l + ϕ 1 ϕ 2 ) J L = sin(θ l + ϕ 1 ϕ 2 ) l cos(θ l + ϕ 1 ϕ 2 ) l cos(θ l + ϕ 1 ϕ 2 ). (2.115) 0 0 1 90

l [mm] 8 7 6 5 4 3 2 1 Rysunek 2.51. Niepewność odległości l jako funkcja ustawienia robota względem landmarku 0 4000 3000 2000 1000 y [mm] 0-1000 -2000-3000 -4000 0 500 1000 1500 2000 2500 x [mm] 3000 3500 4000 pole elipsy niepewnoœci [mm ] x 10 4 7 6 5 4 3 2 1 4000 0 3000 2000 1000 y [mm] 0-1000 -2000-3000 -4000 2000 250030003500 0 500 10001500 x [mm] 4000 [ o ] 2 A B 0.8 0.7 0.6 0.5 0.4 0.3 0.2 0.1 4000 0 3000 2000 1000 0-1000 y [mm] -2000-3000 -4000 4000 3500 2000 25003000 0 500 10001500 x [mm] Rysunek 2.52. Niepewność położenia (A) i orientacji (B) jako funkcja ustawienia robota względem landmarku Na rysunku 2.52A przedstawiono mapę niepewności estymaty położenia robota [ˆx R ŷ R ] T w zależności od jego ustawienia względem obserwowanego landmarku. Za miarę niepewności położenia przyjęto pole elipsy niepewności wyznaczonej na podstawie macierzy C R (2.113). Z kolei na rysunku 2.52B przedstawiono mapę niepewności orientacji robota pozycjonowanego za pomocą landmarku. Przyjętą miarą niepewności orientacji jest odchylenie standardowe σ θ.

Rozdział 3 Przetwarzanie i gromadzenie danych uzyskanych z sensorów 3.1. Reprezentacje informacji przestrzennej w systemie nawigacji robota 3.1.1. Sposoby reprezentacji modelu otoczenia Podejmowanie decyzji (sterowanie) w systemie nawigacji autonomicznego robota mobilnego wymaga odwoływania się do informacji przestrzennych, opisujących otaczające go środowisko. Informacje te gromadzone są przez robota w postaci modelu otoczenia, nazywanego mapą. Sposób tworzenia mapy w dużej mierze zależy od wykorzystanych do tego celu sensorów, a także od zadań stawianych systemowi sterowania robota mobilnego. Jest to przyczyną bardzo dużej różnorodności spotykanych w literaturze przedmiotu podejść do budowy modelu otoczenia. Wspólną cechą wszystkich metod budowy modelu otoczenia jest konieczność przekształcenia surowych danych, odczytanych z sensorów i będących niepewną, niepełną i często niejednoznaczną informacją o otoczeniu, w reprezentację tego otoczenia użyteczną z punktu widzenia systemu nawigacji robota. Za odzwierciedlające obecny stan wiedzy w zakresie tworzenia map otoczenia robotów mobilnych [307] uważa się takie metody, które nie wymagają znajomości pozycji robota z niezależnego źródła, czyli umożliwiają jednoczesne pozycjonowanie i tworzenie mapy (ang. Simultaneous Localization and Mapping SLAM) 1. Algorytmy SLAM opierają się na metodach matematycznej reprezentacji informacji niepewnej i sposobach przetwarzania tego rodzaju informacji. Dominuje podejście probabilistyczne [305], lecz badane są także inne metody reprezentacji informacji, takie jak teoria zbiorów [88] i zbiory rozmyte [113]. W niniejszym rozdziale analizie poddano jedynie sposoby wyodrębniania użytecznej informacji z danych uzyskanych z sensorów oraz algorytmy tworzenia mapy, gdy pozycja robota x R = [x R y R θ R ] T jest znana. Rozszerzenie proponowanych metod przetwarzania informacji sensorycznej do postaci pełnego algorytmu SLAM zaprezentowano w rozdziale 4.2. W przedstawionych tu rozważaniach ograniczono się do płaskich (2D) reprezentacji otocze- 1 Spotykane jest też określenie CML ang. Concurrent Mapping and Localization. 92

nia, gdyż dla nawigacji robotów mobilnych w pomieszczeniach zamkniętych reprezentacja taka jest wystarczająca, a jednocześnie jest znacznie prostsza niż mapy 3D. W literaturze można wyróżnić dwa zasadniczo różne podejścia do problemu reprezentacji danych o otoczeniu. Modele topologiczne są uzyskiwane przez przypisanie identyfikatorów poszczególnym stanom otoczenia postrzeganym przez robota (np. w postaci określonej konfiguracji obserwowanych obiektów). Model topologiczny jest zbiorem rozpoznanych miejsc o określonych, z punktu widzenia percepcji robota, właściwościach oraz powiązań między nimi. Modele geometryczne w jawny sposób reprezentują geometrię i wymiary środowiska, w którym działa robot. Powszechnie stosowane są dwa rodzaje reprezentacji geometrycznych: rastrowe i wektorowe. Mapy rastrowe uzyskuje się przez podział (teselację) powierzchni środowiska operacyjnego robota na regularne komórki (zazwyczaj kwadratowe). Każda komórka reprezentuje stan obszaru o określonych wymiarach fizycznych. Komórkom tym przypisywane są liczby określające stopień ich zajętości przez obiekty (przeszkody). Mapy wektorowe są zbiorami struktur geometrycznych zwanych cechami (ang. feature). Mogą to być punkty, linie proste, odcinki, wieloboki. Cechy opisywane są za pomocą wektorów parametrów określających ich współrzędne w przyjętym układzie odniesienia. Podejście topologiczne umożliwia budowę bardzo zwięzłego, abstrakcyjnego modelu otoczenia [173]. Modele topologiczne są często źródłem danych do globalnego planowania drogi robota współistnieją wówczas w systemie nawigacji z modelem geometrycznym danego środowiska [117]. Mapa topologiczna może też opisywać zgrubnie rozległe otoczenie, którego fragmenty, najbardziej istotne z punktu widzenia nawigacji robota, reprezentowane są w sposób geometryczny [310]. Granica między podejściem topologicznym a geometrycznym nie zawsze jest ostra, gdyż wykorzystywane w rzeczywistych robotach mobilnych mapy topologiczne zazwyczaj zawierają także informacje geometryczne. W praktyce mapy topologiczne charakteryzują się jednak znacznie mniejszą dokładnością reprezentacji geometrii otoczenia niż mapy geometryczne. Nie zawierają one informacji niezbędnych do lokalnego planowania ścieżki i omijania przeszkód. Także samolokalizacja z wykorzystaniem map topologicznych stwarza trudności w związku z małą liczbą szczegółów środowiska reprezentowanych na mapie, co utrudnia rozróżnianie pozycji robota na podstawie odczytów z sensorów 1. Samolokalizacja realizowana na podstawie mapy topologicznej jest zgrubna w sensie rozdzielczości przestrzennej ograniczonej poziomem szczegółowości mapy [266]. Modele wektorowe składają się z cech geometrycznych, takich jak punktowe landmarki [90], odcinki utworzone na podstawie odczytów odległości ze skanerów laserowych [119, 316] lub sonarów [80]. Cechy geometryczne mogą też przyjmować formy specyficzne dla danego rodzaju sensora, takie jak obszary stałego odczytu odległości (RCD ang. Region of Constant Depth) wykorzystywane jako cechy na mapie tworzonej przez robota wyposażonego w sonary [181]. Złożoność modeli wektorowych także może być bardzo różna od minimalistycznych modeli zawierających jedynie ściany pomieszczeń, zapisywane w postaci 1 Problem ten znany jest pod nazwą perceptual aliasing. 93

odcinków [141] lub nieskończonych linii [22], poprzez wieloboki [271], aż po złożone obiekty, podobne do używanych w systemach CAD [75]. Jedną z głównych zalet parametrycznej reprezentacji informacji przestrzennej o otoczeniu robota jest oszczędność zasobów systemowych, co jest szczególnie zauważalne w opisie dużych obszarów. Pozwala to na zastosowanie mapy wektorowej w planowaniu ścieżki, pozycjonowaniu i rozpoznawaniu obiektów za pomocą efektywnych algorytmów. Do tworzenia map wektorowych można też stosunkowo łatwo wykorzystać dane z zasobów CAD (np. plany budynków), które mogą stanowić wiedzę aprioryczną [274]. Najistotniejszą właściwością mapy wektorowej wydaje się jednak jawna reprezentacja poszczególnych obiektów w otoczeniu robota, co pozwala określać ich relacje geometryczne, manipulować nimi na mapie z zachowaniem istotnych cech (kształtu, wymiarów) [274] lub wnioskować o zmianie ich stanu (np. otwarte/zamknięte drzwi) [255]. Budowa wiarygodnego modelu wektorowego na podstawie niepełnych i niepewnych danych jest jednak trudna, gdyż często nie ma możliwości identyfikacji i usunięcia z mapy elementów powstałych na skutek błędnej interpretacji danych z sensorów, spowodowanej niepewnością co do przedmiotu pomiaru. Przypisanie skalarnej miary niepewności do komórek o określonych wymiarach na mapach rastrowych pozwala reprezentować zarówno niepewność geometryczną (co do położenia obiektów na mapie), jak i niepewność co do przedmiotu pomiaru. Wielkość komórek na mapie rastrowej zależy od rozdzielczości zastosowanych sensorów oraz założonego stopnia precyzji odtworzenia kształtów otoczenia. Najczęściej są stosowane komórki o wymiarach od 20 20 cm dla danych uzyskanych z sonarów do 1 1 cm dla danych otrzymanych z sensorów laserowych. Należy zaznaczyć, że koncepcja rastrowego modelu otoczenia [213] powstała na potrzeby przetwarzania danych z sonarów, które charakteryzują się małą wiarygodnością pomiarów (por. p. 2.3). Podstawową zaletą tej metody jest prostota reprezentacji niepewności danych z sensorów oraz możliwość opisu dowolnych środowisk, również słabo ustrukturalizowanych [320]. Wadą jest natomiast nieefektywna w sensie pamięciowym reprezentacja mapy, co czyni model rastrowy mało przydatnym do opisu rozległych środowisk i utrudnia wykorzystanie mapy, np. w celu samolokalizacji [253]. Ze względu na wymienione tu zalety oraz łatwość implementacji mapy rastrowe są najczęściej stosowaną formą gromadzenia informacji przestrzennej w systemach nawigacji robotów [167, 217]. Do aktualizacji wartości komórek map rastrowych wykorzystywane są sformalizowane, matematyczne metody przetwarzania informacji niepewnej, jednak z literatury znane są też algorytmy oparte na heurystycznych regułach aktualizacji wartości komórek [49]. 3.1.2. Przetwarzanie informacji niepewnej w metodach budowy modelu otoczenia Dla robota autonomicznego narzędziami zbierania informacji o otoczeniu są sensory. W związku z tym w metodach stosowanych do budowy modelu otoczenia wykorzystuje się dane nieprecyzyjne: niepełne, niepewne i niejednoznaczne. Wybór sposobu reprezentacji otoczenia na mapie robota autonomicznego powiązany jest z wyborem odpowiedniej dla danego rodzaju mapy metody przetwarzania niepewnych danych. Do tworzenia map rastrowych wykorzystuje się metody probabilistyczne oparte na regule Bayesa [102, 214], a także teorię Dempstera-Shafera [230, 309, 217] i zbiory rozmyte [228]. Z literatury znane są eksperymentalne porównania tych metod stosowanych do przetwarzania danych z dalmierzy ultradźwiękowych [111, 247]. Znane są też zastosowania map rastrowych do przetwarzania danych ze skanerów laserowych, najczęściej wykorzystuje się tu podejście 94

bayesowskie [196, 243, 254, 304]. W rozdziale 3.2 niniejszej pracy przedstawiono i porównano metody tworzenia map rastrowych oparte na teorii Bayesa, teorii Dempstera-Shafera i teorii zbiorów rozmytych. W tym celu posłużono się rzeczywistymi danymi z dwóch różnych skanerów laserowych i zestawu sonarów. W raporcie [264] przedstawiono także wyniki badania heurystycznej metody HIMM Histogramic In-Motion Mapping [49]. Uznano ją jednak za przydatną jedynie w charakterze ściśle lokalnej reprezentacji otoczenia, wspomagającej sterowanie odruchowe robota [63]. Ogólny schemat metody tworzenia mapy rastrowej z wykorzystaniem danych otrzymanych z sensorów odległości jest stały, niezależnie od używanej reprezentacji niepewności. Polega on na zdefiniowaniu modelu sensora służącego do określania, w jakim stopniu dana komórka mapy jest zajęta lub wolna w zależności od wartości odczytu odległości, a następnie na aktualizowaniu stopnia niepewności komórek zgodnie z przyjętym formalizmem matematycznym na podstawie tego modelu i informacji zgromadzonej już na mapie. Każda komórka jest aktualizowana niezależnie od innych. W tworzeniu map wektorowych dominuje podejście probabilistyczne. Cechy są reprezentowane przez wektory parametrów będących zmiennymi losowymi o rozkładach normalnych [289]. Niepewność umiejscowienia danej cechy w geometrycznej przestrzeni wyrażana jest najczęściej za pomocą macierzy kowariancji. Podstawowym narzędziem matematycznym służącym do łączenia niepewnych informacji (fuzji cech) jest filtr Kalmana. Algorytm filtracji Kalmana (ang. Kalman Filter) jest optymalną metodą estymacji stanu systemów liniowych poddanych zakłóceniom gaussowskim [12, 30]. Ponieważ w rozważanym zagadnieniu tworzenia mapy model pomiaru jest zazwyczaj nieliniowy, stosuje się rozszerzony filtr Kalmana (ang. Extended Kalman Filter), w którym dokonywana jest linearyzacja modelu pomiaru w pobliżu punktu pracy systemu. Propagacja niepewności polega na określeniu macierzy kowariancji C y zmiennej y (wektora reprezentującego cechę lub jej określone atrybuty), która to zmienna jest wyznaczana na kolejnym etapie tworzenia mapy jako funkcja innej zmiennej x, y = f(x) (reprezentacji cech lub parametrów na poprzednim etapie przetwarzania), o znanej macierzy kowariancji C x. Ponieważ równania opisujące związki pomiędzy zmiennymi na poszczególnych etapach przetwarzania danych i wyodrębniania cech są najczęściej nieliniowe, wykorzystuje się ich rozkład w szereg Taylora. Jeżeli transformacja zmiennych (różnych opisów) jest dana w postaci jawnej, to można wykorzystać zasadę propagacji macierzy kowariancji [21, 289]: C y = J f C x J T f, (3.1) gdzie J f = f jest jakobianem funkcji f względem wektora x. Jeżeli funkcja wiążąca ˆx x zmienne y i x nie jest dana explicite, to powyższa metoda nie znajduje zastosowania. Sytuacja taka zachodzi wówczas, gdy wartości y uzyskuje się w wyniku użycia metod numerycznych lub np. procedury minimalizacji określonego kryterium. W takich przypadkach można wykorzystać metodę propagacji wariancji zaproponowaną w pracy [124]. Z literatury znane są też przykłady zastosowania zbiorów rozmytych do reprezentacji niepewności na mapie wektorowej złożonej z cech będących odcinkami, a tworzonej na podstawie danych z sonarów [112, 113]. Przydatność zbiorów rozmytych jako reprezentacji niepewności na mapach wektorowych badano także w pracy [286], wykorzystując dane ze skanerów laserowych. Tworzenie mapy wektorowej jest procesem wieloetapowym, w którym można wyróżnić względnie niezależne warstwy, powiązane w hierarchiczną strukturę przepływem strumieni danych o rosnącym stopniu abstrakcji [271]. Czyni to paradygmat tworzenia mapy wekto- 95

rowej zbliżonym do warstwowego schematu przetwarzania informacji i generowania opisu sceny wykorzystywanego w przetwarzaniu obrazów. 3.2. Mapy rastrowe 3.2.1. Metoda probabilistyczna Wykorzystanie reguły Bayesa do tworzenia mapy Teoria prawdopodobieństwa jest najstarszą metodą interpretacji informacji niepewnej. Do przetwarzania informacji sensorycznej przydatna jest aksjomatyczna definicja prawdopodobieństwa, oparta na pewnikach [315]. Ta interpretacja, nazywana subiektywną, uznaje prawdopodobieństwo za miarę odczuwania niepewności za pewną liczbę charakterystyczną dla danego zdarzenia losowego [47]. Tak zdefiniowane prawdopodobieństwo może być rozumiane jako stopień przekonania o wystąpieniu zdarzenia A. W przypadku robota przekonanie to wynika z ewidencji uzyskanej z danych otrzymanych z sensorów lub w niektórych przypadkach jest dane a priori i odzwierciedla wiedzę projektanta systemu. Prawdopodobieństwo warunkowe (ang. conditional probability) jest interpretowane jako stopień przekonania o wystąpieniu określonego zdarzenia A, należącego do zbioru (przestrzeni) zdarzeń elementarnych A Ω, pod warunkiem zajścia innego zdarzenia B Ω. Jest wyrażane wzorem: P(B A) P(A B) =. (3.2) P(B) W podejściu probabilistycznym do przetwarzania informacji niepewnej podstawową rolę odgrywa twierdzenie Bayesa [47]. Na mocy tego twierdzenia można określić prawdopodobieństwo prawdziwości danej hipotezy h pod warunkiem uzyskania informacji e: P(h e) = P(e h)p(h). (3.3) P(e) Oznaczywszy rastrową mapę otoczenia o wymiarach n i n j komórek przez M, a zbiór m dostępnych obserwacji (pomiarów) przez E = {e 1,...e m }, problem tworzenia mapy otoczenia (przy założeniu znanej pozycji robota) można rozważać jako problem wyznaczenia warunkowego prawdopodobieństwa zajętości wszystkich komórek tej mapy w zależności od dostępnych obserwacji: P(M E). W praktyce liczba komórek na mapie równa n i n j może być bardzo duża, co powoduje, że wyznaczenie prawdopodobieństwa a posteriori staje się nieefektywne obliczeniowo. Uproszczeniem powszechnie przyjmowanym w algorytmach tworzenia map rastrowych (nie tylko tych wykorzystujących podejście probabilistyczne) jest założenie o niezależności stanu poszczególnych komórek. W formalizmie probabilistycznym odpowiada to wyznaczeniu warunkowego prawdopodobieństwa zajętości danej komórki mapy M[i, j] w zależności od znanych obserwacji: P(M[i, j] E). W ten sposób wielowymiarowy problem estymacji zostaje zastąpiony n i n j problemami jednowymiarowymi, rozwiązywanymi niezależnie dla każdej komórki mapy. W praktyce założenie o niezależności poszczególnych komórek nie zawsze jest spełnione. Dzieje się tak, gdy dana obserwacja dotyczy więcej niż jednej komórki oraz gdy prawdopodobieństwa zajętości poszczególnych komórek są ze sobą związane. Przykładem obserwacji zmieniających prawdopodobieństwo wielu komórek są pomiary dalmierzami ultradźwiękowymi modyfikowane jest prawdopodobieństwo zajętości komórek leżących na całej szerokości kątowej wiązki, podczas gdy obiekt, 96

który spowodował odczyt, może się znajdować tylko w obszarze jednej komórki. Zależności między komórkami mapy wynikają też ze struktury otoczenia, które składa się z obiektów reprezentowanych zazwyczaj przez grupy wielu komórek. Jednak aktualizacja pewnej komórki mapy nie implikuje zmian w pozostałych komórkach reprezentujących ten sam obiekt. Tworzenie mapy rastrowej można rozumieć jako poszukiwanie mapy M najlepiej wyjaśniającej posiadane obserwacje E. Założenie o niezależności komórek powoduje jednak, że w procesie tworzenia mapy wyznaczane jest prawdopodobieństwo a posteriori zajętości każdej komórki, które najlepiej wyjaśnia obserwacje E dla tej komórki, jednak bez uwzględnienia pozostałych komórek. Może to prowadzić do powstawania sprzeczności na mapie [308]. Sensory odległości sonary i dalmierze lub skanery laserowe dostarczają informacje o otoczeniu pozwalające stwierdzić, czy dany obszar przestrzeni odpowiadający komórce M[i, j] mapy rastrowej jest zajęty, czy wolny. Niepewność związana z każdym pomiarem odległości powoduje jednak, że nie jest to rozstrzygnięcie jednoznaczne. Odczyt z sensora to jedynie określona ewidencja wspierająca jedną z hipotez: komórka jest wolna lub komórka jest zajęta. Dopuszczalne stany komórki można zapisać jako parę dopełniających się hipotez: H(M[i, j]) = {occupied, empty}. (3.4) Z aksjomatów teorii prawdopodobieństwa wynika dopełnianie się prawdopodobieństw tak zdefiniowanych hipotez: P(empty) = 1 P(occupied). (3.5) Model sensora określa prawdopodobieństwo warunkowe odczytu o wartości r, gdy dana komórka mapy jest zajęta przez obiekt. Prawdopodobieństwo to jest zapisywane jako P(r occ). Z punktu widzenia tworzenia mapy interesujące jest jednak prawdopodobieństwo P(occ r), że dana komórka jest zajęta, gdy otrzymano z sensora odczyt r. Relację między tymi dwoma prawdopodobieństwami określa reguła Bayesa, którą, używając oznaczeń 1 jak we wzorze (3.4), można zapisać jako: P(occ r) = P(r occ)p(occ) P(r occ)p(occ) + P(r emp)p(emp), (3.6) gdzie P(occ) i P(emp) są prawdopodobieństwami a priori zajścia danego zdarzenia. Prawdopodobieństwo, że komórka jest zajęta, gdy otrzymano n pomiarów r 1,r 2,...r n wyrażone jest zależnością: P(occ r 1,...r n ) = P(r 1,r 2...r n occ)p(occ) P(r 1,r 1...r n occ)p(occ) + P(r 1,r 1...r n emp)p(emp). (3.7) Korzystając z własności niezależności obserwacji, w regule Bayesa postaci (3.7) prawdopodobieństwo P(r 1,r 1...r n occ) można zastąpić iloczynem P(r 1 occ)p(r 2 occ)...p(r n occ). Uwzględniając, że P(occ r)p(r) = P(r occ)p(occ) oraz biorąc pod uwagę własność (3.5), otrzymuje się iteracyjną regułę agregacji pomiarów wyrażoną wzorami: P(r occ)p(occ m) P(occ r) = P(r occ)p(occ m) + P(r emp)p(emp m), (3.8) P(emp r) = 1 P(occ r), (3.9) 1 Dla zwięzłości zapisu przyjęto skróty occ i emp. 97

gdzie P(occ m) i P(emp m) to wartości prawdopodobieństwa odczytane z aktualizowanej komórki mapy, a P(r occ) i P(r emp) to wartości prawdopodobieństwa otrzymane z modelu sensora. Powyższy wzór jest słuszny jedynie przy założeniu, że kolejne agregowane za jego pomocą pomiary są niezależne. To założenie w niektórych sytuacjach nie jest spełnione, np. jeżeli robot w statycznym otoczeniu wykonuje wiele pomiarów w jednej pozycji, to kolejne odczyty nie niosą nowej informacji o stanie komórek mapy. Stosowanie reguły Bayesa przy założeniu warunkowej niezależności obserwacji może prowadzić do błędnych ocen prawdopodobieństwa hipotez, jeżeli między obserwacjami występują nawet niewielkie zależności [129]. Z literatury znane są próby zapewnienia niezależności obserwacji przez zapamiętywanie pomiarów lub pozycji robota, z których pomiary te zostały wykonane [165]. Metody te są jednak efektywne jedynie w przypadku niewielkich map ze względu na konieczność przechowywania dużej ilości danych w poszczególnych komórkach. Wzory (3.8) i (3.9) są używane do tworzenia mapy w sposób rekurencyjny, obliczone dla danej komórki prawdopodobieństwo P(occ r) jest przechowywane w niej jako nowa wartość P(occ m), natomiast P(emp m) = 1 P(occ m) nie jest zapisywane na mapie. Na początku procesu tworzenia mapy wartości P(occ m) nie są znane. Zazwyczaj przyjmowana jest dla nich wartość P(occ m) = 0, 5, odpowiadająca niewiedzy (maksymalna entropia [129]). Jeżeli dostępna jest określona informacja o danym otoczeniu, np. w postaci danych z systemu CAD, to można przypisać początkową wartość większą niż 0,5 komórkom mapy odpowiadającym obszarom zajętym przez nieruchome obiekty [264]. Traktowanie braku informacji jako równego prawdopodobieństwu występowania wykluczających się hipotez P(occ m) = P(emp m) = 0,5 jest istotną wadą. Wynika ona z braku reprezentacji niewiedzy oraz niewykrywania informacji sprzecznych, co jest cechą podejścia bayesowskiego do przetwarzania informacji niepewnej [47]. Reguła Bayesa zastosowana do agregacji pomiarów nie pozwala odróżnić sytuacji, w której o danej komórce nie zgromadzono żadnej informacji (obszar nie był eksplorowany), od sytuacji, gdy zgromadzono taką samą liczbę równoważnych pomiarów świadczących o tym, że komórka jest zajęta, i wskazujących, że jest ona wolna (sprzeczne dane). Modele sensorów Istotnym elementem opisywanej metody jest model sensora, odzwierciedlający niepewność informacji o występowaniu przeszkody w obszarze danej komórki mapy w przypisywaną jej wartość prawdopodobieństwa. Poniżej przedstawiono modele (zwane modelami wiązki) sensorów odległości używanych w Laboratorium Robotów Mobilnych IAiII: sonarów i skanerów laserowych. Dostępną estymatą odległości mierzonej dalmierzem ultradźwiękowym jest bieżący odczyt r m. Przy założeniu, że błędy systematyczne zostały wyeliminowane przez zastosowanie korekcji (2.43), niepewność pomiaru r zależy od składnika losowego i wyrażana jest wzorem (2.44). Zależność odchylenia standardowego σ r od zmierzonej odległości dana jest równaniem (2.45). Odwzorowując te zależności w modelu wiązki sonaru, przyjęto, że prawdopodobieństwo występowania obiektu, od którego odbił się impuls ultradźwiękowy, jest opisane krzywą Gaussa o wartości maksymalnej p o w odległości r od sonaru. Zgodnie z modelem (2.44) obszar, w którym z prawdopodobieństwem równym 0,99 występuje obserwowana przeszkoda, zamyka się w przedziale ± 3σ r wokół odległości r. Pomiar odległości o wartości r implikuje, że z określonym prawdopodobieństwem p e obszar między sonarem a punktem 98

domniemanego występowania przeszkody jest pusty. Stan komórek mapy znajdujących się w odległości większej niż r + 3σ r od sonaru jest nieznany. Powyższy model, przedstawiający charakterystykę sonaru w funkcji odległości, jest opisany jako: p e 0 ρ < r 2 r, ( ) ) 2 p e (1 ρ r+2 r r r 2 r ρ < r r, α s (ρ) = (ρ r) 2 1 p o e 2σr 2(r) 2πσr(r) r r ρ < r + r, 0 ρ r + r, (3.10) gdzie: ρ odległość od sonaru do środka rozpatrywanej komórki, r = 3σ r przedział wokół r, w którym może występować obserwowany obiekt. Dla obszaru bezpośrednio przed obiektem, r 2 r ρ < r r, wprowadzono zależność zapewniającą płynne przejście między obszarem prawdopodobnie wolnym a prawdopodobnie zajętym. Współczynniki 0 < p o < 1 i 0 < p e < 1 pozwalają na skalowanie maksymalnych wartości uzyskiwanego prawdopodobieństwa w zależności od rodzaju środowiska. W środowiskach, w których powstaje wiele odbić zwierciadlanych, można zmniejszyć wartości p o i p e. Wówczas otrzymuje się bardziej zachowawczy (konserwatywny) model wiązki. Współczynniki te nie pozwalają też na przypisanie komórce mapy prawdopodobieństwa P(occ m) = 1 (z pewnością zajęta) lub P(occ m) = 0 (z pewnością wolna), gdyż reguła Bayesa nie pozwala na rewizję tak ustalonego prawdopodobieństwa a priori, niezależnie od prawdopodobieństwa wskazywanego w kolejnych pomiarach [47]. Sonary charakteryzują się dużą szerokością wiązki pomiarowej. Pomiar może wskazywać na występowanie obiektu gdziekolwiek w zakresie tej szerokości. Wiarygodny pomiar odległości jest jednak bardziej prawdopodobny, gdy przeszkoda znajduje się blisko osi akustycznej sonaru (por. rys. 2.12). Niepewność położenia kątowego obiektu wewnątrz wiązki jest opisana krzywą Gaussa, zgodnie ze wzorem (2.46). Odchylenie standardowe σ θ wynosi θ son 2 (wzór 2.40). Prawdopodobieństwo wykrycia obiektu w zależności od odchylenia kątowego względem osi akustycznej sonaru opisuje funkcja β s (ϕ): ϕ 2 1 2σ β s (ϕ) = 2πσθ e θ 2 ϕ θ son, (3.11) 0 ϕ > θ son, gdzie ϕ to kąt między osią akustyczną wiązki a odcinkiem łączącym sonar ze środkiem rozpatrywanej komórki. Przedstawiony w podrozdziale 2.3 model pomiarowy dalmierza ultradźwiękowego wskazuje, że dominującym rodzajem niepewności jest niepewność co do przedmiotu pomiaru. Prawdopodobieństwo przechwycenia przez sonar niewłaściwego echa rośnie wraz z czasem oczekiwania na jego powrót, a więc wraz z mierzoną odległością. Ze wzrostem tej odległości następuje także osłabienie odbieranego echa (wzór 2.41). Zjawisko to znajduje odzwierciedlenie w modelu wiązki pomiarowej przez wprowadzenie funkcji γ s (ρ), która odwzorowuje w przybliżony sposób osłabienie echa i powoduje, że wyznaczane prawdopodobieństwo maleje wraz ze wzrostem mierzonej odległości, aż do osiągnięcia zera dla r max = 5 m: γ s (ρ) = ( 1 ρ 2 (r max r) 2 ). (3.12) 99

Funkcja opisująca prawdopodobieństwo warunkowe odczytu r, gdy dana komórka mapy jest zajęta, powstaje przez złożenie modeli cząstkowych (3.10), (3.11) i (3.12) oraz sprowadzenie do zakresu (0,1). Prawdopodobieństwo warunkowe odczytu, gdy komórka jest wolna, jest obliczane na podstawie wzoru (3.5): P(r occ) = 0,5α s (ρ)β s (ϕ)γ s (ρ) + 0,5, (3.13) P(r emp) = 1 P(r occ). (3.14) Trójwymiarową wizualizację modelu wiązki opisanego równaniem (3.13) przedstawiono na rys. 3.1. A B P(r occ) 0.9 P(r occ) 0.6 0.4 0.2 0-50 0 50 0 [cm] 50 100 150 200 250 300 350 400 450 500 0.8 0.7 0.6 0.5 0.4 0.3 0.2 0.1 0-20 0 20 0 [cm] 20 40 60 80 100 120 140 160 180 200 Rysunek 3.1. Probabilistyczny model wiązki dalmierza ultradźwiękowego; A odległość 4,5 m, B odległość 1,7 m Skanery laserowe charakteryzują się właściwościami istotnie różnymi od sonarów (por. p. 2.4.2 i p. 2.4.3). Wymaga to wprowadzenia dla tego rodzaju sensorów odległości odmiennych funkcji odzwierciedlających niepewność informacji o występowaniu przeszkody w obszarze wiązki. Pomimo odmiennej zasady pomiaru odległości i różnych parametrów użytkowych proponowane modele niepewności pomiaru skanerem KARiI i skanerem LMS 200 są takie same, różnią się jedynie wartościami parametrów i sposobem ich wyznaczania. Dlatego w przypadku tych sensorów można korzystać z tego samego modelu wiązki poddanego odpowiedniej dla każdego z nich parametryzacji. Dostępną estymatą mierzonej odległości dla skanera LMS 200 jest bieżący odczyt r m, a dla skanera KARiI średnia z serii pomiarów r m. Pomiary odległości wykraczające poza ustalony zasięg r max są ignorowane jako mało wiarygodne. Dla skanera LMS 200 r max wynosi 7 m, a dla skanera KARiI 4 m. Ponadto, dla skanera KARiI wykonuje się test wiarygodności pomiaru (wzór 2.68). Błędy systematyczne są eliminowane przez zastosowanie zależności (2.82) dla skanera LMS 200 oraz procedury opartej na wykorzystaniu odchylenia standardowego z serii pomiarów (2.71), (2.72) dla skanera KARiI. Po eliminacji błędów systematycznych niepewność pomiaru odległości obu rozpatrywanymi sensorami zależy od składnika losowego. Zgodnie z modelem niepewności (por. rys. 2.32 i 2.45) oraz analogicznie do modelu wiązki sonaru przyjęto, że prawdopodobieństwo występowania obiektu jest opisane krzywą Gaussa. Zależność odchylenia standardowego σ r od zmierzonej odległości dla skanera LMS 200 jest dana równaniem (2.85), a dla skanera KARiI σ r jest wyznaczane na podstawie serii pomiarów. Prawdopodobnie wolny obszar między skanerem a punktem występowania przeszkody opisywany jest identycznie jak dla sonaru. Opis modelu wiązki w funkcji odległości przyjmuje postać: 100

p e 0 ρ < r 2 r, ( ) ) 2 p e (1 ρ r+2 r r r 2 r ρ < r r, α l (ρ) = (ρ r) 2 1 p o e 2σr 2(r) 2πσr(r) r r ρ < r + r, 0 ρ r + r, (3.15) która jest identyczna z funkcją (3.10) dla sonaru. Współczynniki 0 < p o < 1 i 0 < p e < 1 mają taką samą interpretację, jednak ze względu na większą wiarygodność pomiarów skanerem laserowym można przyjąć dla nich wartości bliższe jedności. Wiązka skanera laserowego ma mały kąt rozwarcia. Niepewność wynikająca z wielkości śladu optycznego sprawia, że prawdopodobieństwo powrotu sygnału odbitego od obiektu umiejscowionego pod kątem ϕ względem osi wiązki jest opisane rozkładem jednostajnym, zarówno dla skanera KARiI (wzór 2.80) jak i LMS 200 (wzór 2.87). W związku z tym funkcja określająca prawdopodobieństwo wykrycia obiektu w zależności od odchylenia kątowego względem osi wiązki przyjmuje odmienną niż dla sonaru postać: β l (ϕ) = { 1 ϕ θlas, 0 ϕ > θ las, (3.16) gdzie szerokość kątowa wiązki sensora laserowego θ las przyjmuje odpowiednio wartości: 0,5 dla LMS 200 i 1,5 dla skanera KARiI. Przyjęcie nieco większych wartości θ las niż kąty rozwarcia wiązki obu sensorów (oszacowane w p. 2.4.2 i 2.4.3) uzasadnione jest istnieniem niepewności co do kąta osi wiązki względem układu współrzędnych skanera, wprowadzanej przez mechanizm obrotu zwierciadła. W przeciwieństwie do odczytów sonarów pomiary uzyskane ze skanerów laserowych nie są obarczone istotną niepewnością co do przedmiotu pomiaru. Niepewność taka pojawia się jednak na skutek efektu śladu optycznego. W przypadku skanera KARiI jest ona usuwana dzięki analizie rozrzutu pomiarów w serii (wzór 2.68), natomiast występujące rzadko w pomiarach sensorem LMS 200 mixed pixels są usuwane na kolejnych etapach przetwarzania danych (por. p. 3.3). W przeciwieństwie do sonarów skanery laserowe nie dostarczają błędnych odczytów odległości, znacznie większych niż rzeczywiste, w związku z czym można uznać, że obszar wiązki między sensorem a punktem odległym o r r jest wolny z prawdopodobieństwem p e. Nie ma potrzeby wprowadzania dodatkowej funkcji zmniejszającej szacowane prawdopodobieństwo w pomiarze większych odległości. Funkcja opisująca prawdopodobieństwo warunkowe odczytu powstaje przez złożenie funkcji (3.15) i (3.16) oraz normalizację: P(r occ) = 0,5α l (ρ)β l (ϕ) + 0,5, (3.17) P(r emp) = 1 P(r occ). (3.18) Trójwymiarową wizualizację modelu wiązki opisanego funkcją (3.17) przedstawiono na rys. 3.2: dla pomiaru skanerem KARiI o wartości 2,5 m (A) i dla pomiaru skanerem LMS 200 o wartości 5 m (B). 101

A B P(r occ) 1 0.8 0.6 0.4 0.2 0-20 0 20 [cm] 0 50 100 150 200 250 P(r occ) 1 300 0.5 0-50 0 50 [cm] 0 50 100 150 200 250 300 350 400 450 500 550 Rysunek 3.2. Probabilistyczne modele wiązek skanerów laserowych (opis w tekście) 3.2.2. Metoda oparta na teorii ewidencji Zastosowanie teorii Dempstera-Shafera do tworzenia mapy rastrowej Teoria Dempstera-Shafera (D-S), zwana również matematyczną teorią ewidencji [260], jest metodą alternatywną dla teorii Bayesa, dopuszczającą częściowo wyspecyfikowany model prawdopodobieństwa. Teoria D-S odrzuca aksjomat probabilistyki wymagający, by prawdopodobieństwa hipotez przeciwstawnych się dopełniały. W przeciwieństwie do podejścia bayesowskiego w metodzie tej wyznaczane jest prawdopodobieństwo, z jakim można wykazać prawdziwość danej hipotezy na podstawie posiadanych informacji, a nie prawdopodobieństwo prawdziwości hipotezy [47]. Z punktu widzenia zastosowań w przetwarzaniu danych uzyskanych z sensorów robota podstawową zaletą teorii D-S jest rozróżnienie między niepewnością a niewiedzą. Skończony zbiór zawierający wszystkie zdarzenia elementarne (hipotezy, propozycje) nazywany jest w teorii Dempstera-Shafera ramą rozróżniającą (ang. frame of discerment) i oznaczany Θ. Inaczej niż w podejściu bayesowskim, Θ nie musi się składać z wykluczających się hipotez. Dostępne informacje o elementach A Θ są zapisywane w postaci rozkładu bazowego prawdopodobieństwa, który reprezentuje częściowe przekonania. Każde źródło informacji (np. sensor) dostarcza masę prawdopodobieństwa (ang. belief mass) równą 1, jednak może być ona rozdzielona dowolnie między hipotezy z Θ lub ich kombinacje. W teorii D-S nie wymaga się podziału prawdopodobieństwa między elementarne hipotezy. Zbiór wszystkich możliwych podzbiorów Θ, na które może być rozdzielona masa prawdopodobieństwa, oznaczany jest 2 Θ, gdyż jego liczność wynosi 2 Θ. Zdefiniowana na tym zbiorze funkcja masy prawdopodobieństwa m : 2 Θ [0,1] spełnia warunki: m( ) = 0, m(a) = 1. (3.19) A Θ Wiarygodność przesłanek na rzecz hipotezy A mierzy funkcja przekonania 1 (ang. belief): Bel : 2 Θ [0,1] taka, że dla każdego zbioru A Θ Bel(A) = B A m(b). (3.20) 1 Polskie nazewnictwo zaczerpnięto z książki [47]. 102

Natomiast stopień, w jakim zgromadzona informacja przemawia przeciw A, określa funkcja wyobrażalności (ang. plausibility): Pls : 2 Θ [0,1] taka, że dla każdego zbioru A Θ Pls(A) = m(b). (3.21) A B Zachodzi zależność: Pls(A) = 1 Bel( A). Rzeczywiste prawdopodobieństwo hipotezy A leży między Bel(A), będącą jego kresem dolnym, a Pls(A), interpretowaną jako kres górny. Odcinek [Bel(A), Pls(A)], nazywany przedziałem przekonania, reprezentuje pewność co do prawdziwości hipotezy A. Przedział ten zawęża się w miarę gromadzenia informacji dotyczącej rozpatrywanej hipotezy. Funkcja łączenia niepewności dotyczącej tego samego zbioru jest nazywana regułą kombinacji Dempstera (inna nazwa to suma ortogonalna). Dysponując dwoma rozkładami m 1 i m 2 dla ramy Θ, można dokonać ich połączenia według reguły: m 1 (A)m 2 (B) A B=C C Θ,C m(c) = 1 (3.22) m 1 (A)m 2 (B) A B= i otrzymać nowy rozkład bazowy m. Normalizacja w powyższym wyrażeniu zapewnia spełnienie przez m warunków (3.19). Kombinacji tej nie można dokonać, gdy mianownik wzoru (3.22) jest równy zeru. Sytuacja taka świadczy o całkowitej sprzeczności m 1 i m 2. Problematyczność zastosowania reguły kombinacji Dempstera w sytuacji silnie sprzecznych przesłanek została zauważona przez Shafera [260]. Zaproponował on alternatywną dla reguły (3.22) metodę łączenia rozkładów, opartą na sumie ważonej, w której wagi odzwierciedlają zaufanie do poszczególnych źródeł informacji. O sprzeczności łączonych rozkładów bazowych świadczy część masy prawdopodobieństwa, która nie zostaje przypisana do hipotez A Θ. Zastosowana w regule (3.22) normalizacja oznacza brak wskazania na informacje niezgodne. Shafer [260] zdefiniował miarę niezgodności informacji (ang. weight of conflict). Dla rozkładów m 1 i m 2 można ją zapisać jako: Con(m 1,m 2 ) = log 1 A B= 1 m 1 (A)m 2 (B). (3.23) W porównaniu z metodą probabilistyczną użycie wprowadzonych przez Shafera funkcji przekonania i wyobrażalności oraz podanej przez Dempstera metody łączenia opisów niepewności daje narzędzie do tworzenia mapy otoczenia robota pozwalające lepiej przedstawić obszary, o których zebrano zbyt mało danych lub zebrano dane sprzeczne. Możliwe stany komórki mapy rastrowej M[i, j] opisują hipotezy (3.4), analogiczne do przyjętych dla podejścia bayesowskiego. Ramą rozróżniającą jest więc Θ = {occupied,empty}, a zbiór 2 Θ ma postać: 2 Θ = {,occupied,empty, {occupied,empty}}. (3.24) Zawiera on oprócz hipotez elementarnych (ang. singleton propositions) także zbiór Θ reprezentujący przekonanie, że komórka jest zajęta lub wolna, czyli niewiedzę. Zbiór pusty oznacza, że stanu komórki nie wyjaśnia żadna inna hipoteza. Dla uproszczenia notacji zbiór (3.24) może być przedstawiony jako {,occupied,empty,unknown}. Sensory odległości dostarczają informacje o otoczeniu interpretowane jako masy prawdopodobieństwa m r (emp) i mr (occ) reprezentujące częściowe przekonanie, że dana komórka 103

jest odpowiednio wolna i zajęta. Na mocy założenia (3.19) masa prawdopodobieństwa nie może być przypisana do. Korzystając z tego samego założenia, wartość funkcji m r (unk) odzwierciedlającej niewiedzę w modelu sensora można wyznaczyć na podstawie wzoru: m r (unk) = 1 mr (occ) mr (emp). (3.25) Tworzenie mapy rastrowej polega na łączeniu informacji płynącej z sensorów i znajdującej się już w komórkach z wykorzystaniem reguły kombinacji Dempstera. By dostosować wzór (3.22) do zadania aktualizacji komórek mapy, zapisuje się go w postaci iteracyjnej: (unk) m n (occ) = mn 1 (occ) mr (occ) + mn 1 (occ) mr (unk) + mr (occ) mn 1 1 m n 1 (occ) mr (emp) mn 1 (emp) mr (occ) m n (emp) = mn 1 (emp) mr (emp) + mn 1 (emp) mr (unk) + mr (emp) mn 1 1 m n 1 (occ) mr (emp) mn 1 (emp) mr (occ) m n (unk) = m n 1 (unk) mr (unk) 1 m n 1 (occ) mr (emp) mn 1 (emp) mr (occ), (3.26) (unk), (3.27). (3.28) gdzie m n 1 (occ), mn 1 (emp) i mn 1 (unk) to masy prawdopodobieństwa przechowywane w aktualizowanej komórce mapy, a m n (occ), mn (emp) i mn (unk) nowy rozkład mas prawdopodobieństwa dla tej komórki. Funkcje przekonania Bel i wyobrażalności Pls dla hipotezy occupied wyznaczane są ze wzorów: Bel n (occ) = mn 1 (occ) + mr (occ) + mn 1 (occ) mr (occ) mr (occ) mn 1 (emp) mn 1 1 m n 1 (occ) mr (emp) mn 1 (emp) mr (occ) (emp) mr (emp) Pls n (occ) = 1 + mn 1 (emp) mr (emp) mn 1 1 m n 1 (occ) mr (emp) mn 1 (emp) mr (occ) (occ) mr (emp), (3.29). (3.30) Dla pozostałych hipotez: empty i unknown funkcje te są wyznaczane w analogiczny sposób. Gdy robot rozpoczyna tworzenie mapy rastrowej z wykorzystaniem teorii D-S, komórki tej mapy inicjowane są w sposób odzwierciedlający całkowity brak wiedzy, przez przypisanie: m 0 (occ) = 0, m0 (emp) = 0, m0 (unk) = 1. O prawdziwości danej hipotezy można wnioskować na podstawie stosunku wartości obu funkcji. Na przykład, jeżeli dla hipotezy occupied zarówno Bel, jak i Pls będą mieć dużą wartość, to można uznać prawdziwość tej hipotezy dla danej komórki (obszaru mapy). Jeżeli natomiast dla tej samej hipotezy Bel będzie mieć małą wartość, a Pls dużą, to należy uznać, że ilość informacji zgromadzonych o danym obszarze jest zbyt mała. Istotna jest możliwość wyznaczenia miary niezgodności Con między dwoma źródłami informacji (np. różnymi sensorami). Miara ta może być wykorzystana do oceny jakości danych zebranych z sensorów [69] oraz do przeprowadzenia percepcji aktywnej [216]. Modele sensorów Modele wiązek pomiarowych sensorów odległości wykorzystywane w procedurze tworzenia mapy rastrowej z zastosowaniem teorii Dempstera-Shafera oparto na ogólnych modelach pomiarowych zaprezentowanych w podrozdziałach 2.3 i 2.4. Struktura funkcji opisujących te 104

modele jest analogiczna do struktury przyjętej dla modeli probabilistycznych przedstawionych w podrozdziale 3.2.1. Zasadnicza różnica wynika z tego, że aksjomat (3.5) nie obowiązuje w teorii D-S. Ponieważ prawdopodobieństwa hipotez przeciwstawnych nie muszą się dopełniać, buduje się dwa niezależne modele wiązki, określające odrębne wartości masy prawdopodobieństwa dla hipotez occupied i empty. Model wiązki sonaru uwzględnia niepewność zmierzonej odległości, która opisana jest rozkładem normalnym o odchyleniu standardowym σ r. Obszar między sensorem a miejscem, w którym wykryto przeszkodę, uznawany jest za wolny: 0 0 ρ < r r, (ρ r) 2 α o (ρ) = 1 k o e 2σr 2(r) 2πσr(r) r r ρ < r + r,, (3.31) 0 ρ r + r, k e 0 ρ < r r, ( α e (ρ) = k r ρ ) 2 e r r r ρ < r,, (3.32) 0 ρ r, gdzie ρ jest odległością od sensora do środka danej komórki, a r = 3σ r i jest przedziałem wokół r, w którym może występować obserwowany obiekt. Współczynniki k o i k e są granicami α o i α e takimi, że k o +k e < 1. Analogicznie jak w modelu probabilistycznym, pozwalają one na skalowanie maksymalnych wartości uzyskiwanej masy prawdopodobieństwa. Wartości współczynników mniejsze od jedności zabezpieczają procedurę tworzenia mapy przed sytuacją, w której suma ortogonalna (3.22) jest nieokreślona. Prawdopodobieństwo wykrycia obiektu w zależności od odchylenia kątowego względem osi akustycznej sonaru opisuje funkcja β s (ϕ) postaci (3.11), identyczna z funkcją w modelu probabilistycznym. Także niepewność co do przedmiotu pomiaru modelowana jest analogicznie jak w poprzednim przypadku i opisana funkcją γ s (ρ) postaci (3.12). Po uwzględnieniu wszystkich składników niepewności wartości masy prawdopodobieństwa dla modelu wiązki sonaru są wyznaczane w następujący sposób: m r (occ) = α o(ρ)β s (ϕ)γ s (ρ), (3.33) m r (emp) = α e(ρ)β s (ϕ)γ s (ρ), (3.34) m r (unk) = 1 mr (occ) mr (emp). (3.35) Należy zauważyć, że w tym przypadku prawdopodobieństwo pozostałe po uwzględnieniu proponowanego modelu wiązki sensora jest przydzielane hipotezie unknown, co oznacza brak informacji z sonaru. Na rysunku 3.3 przedstawiono trójwymiarową wizualizację proponowanego modelu wiązki sonaru w postaci funkcji określającej masę prawdopodobieństwa dla hipotezy occupied (A) i dla hipotezy empty (B). Model wiązki skanera laserowego, wykorzystywany do tworzenia mapy opartej na teorii D-S, powstaje analogicznie jak powyższy model dla sonaru. Funkcja odzwierciedlająca niepewność pomiaru odległości, dana wzorami (3.31) i (3.32), jest poddawana parametryzacji przez dobór wartości k o i k e oraz wyznaczenie odchylenia standardowego σ r na podstawie modelu matematycznego (dla LMS 200) lub serii pomiarów (dla skanera KARiI). Funkcja β l (ϕ) określająca prawdopodobieństwo wykrycia obiektu w zależności od odchylenia kątowego względem osi wiązki przyjmuje właściwą dla sensorów laserowych postać (3.16). 105

A B m, o o m e, e 0.6 1 0.4 0.2 0 50 0-50 300 250 200 150 100 50 0 [cm] 0.8 0.6 0.4 0.2 0 50 0-50 300 250 200 150 100 50 0 [cm] Rysunek 3.3. Model wiązki dalmierza ultradźwiękowego używany w metodach D-S i rozmytej; A hipoteza occupied, B hipoteza empty, mierzona odległość 2,5 m W przeciwieństwie do modelu wiązki ultradźwiękowej nie występuje funkcja zmniejszająca szacowane masy prawdopodobieństwa hipotez przy pomiarze większych odległości. Masy prawdopodobieństwa dla modelu wiązki skanera laserowego są wyznaczane w następujący sposób: m r (occ) = α o(ρ)β l (ϕ), (3.36) m r (emp) = α e(ρ)β l (ϕ), (3.37) m r (unk) = 1 mr (occ) mr (emp). (3.38) Na rysunku 3.4 przedstawiono trójwymiarową wizualizację modelu wiązki skanera laserowego LMS 200 w postaci funkcji określającej masę prawdopodobieństwa dla hipotezy occupied (A) i dla hipotezy empty (B). A B m o, m e, o 1 1 e 0.8 0.8 0.6 0.6 0.4 0.2 0 20 0-20 200 180 160 140 120 100 80 60 40 20 0 [cm] 0.4 0.2 0 20 0-20 200 180 160 140 120 100 80 60 40 20 0 [cm] Rysunek 3.4. Model wiązki skanera laserowego używany w metodach D-S i rozmytej; A hipoteza occupied, B hipoteza empty, mierzona odległość 1,7 m 106

3.2.3. Metoda oparta na zbiorach rozmytych Reprezentacja niepewności za pomoca zbiorów rozmytych Teoria zbiorów rozmytych jest formalizmem matematycznym umożliwiającym reprezentację niepewności co do określonych zdarzeń lub hipotez. Teoria ta pozwala na wyrażenie za pomocą aparatu matematycznego pojęć o charakterze jakościowym, takich jak mały,duży, bardzo szybki itp. [47]. Zbiory rozmyte mogą być rozpatrywane jako rozszerzenie klasycznych zbiorów o ostrych granicach. Dla danego zbioru rozmytego X w dziedzinie U definiuje się funkcję przynależności do tego zbioru µ X : U [0,1], która może przybierać dowolne wartości pomiędzy 0 a 1, co pozwala reprezentować nieostrą (rozmytą) granicę rozdzielającą elementy należące do zbioru X od elementów, które do niego nie należą. Tak określone zbiory rozmyte pozwalają nie tylko reprezentować brak precyzji charakterystyczny dla języka naturalnego, lecz nadają się także do reprezentacji niepewności w zastosowaniach technicznych. Przez dobieranie odpowiednich funkcji przynależności można reprezentować różne rodzaje niepewności związane z pomiarami lub identyfikacją parametrów obserwowanych obiektów, takie jak nieokreśloność, brak precyzji i wieloznaczność, których opis innymi metodami jest trudny. Zbiory rozmyte oraz oparty na nich aparat wnioskowania logika rozmyta znalazły wiele zastosowań w systemach sterowania [100], w tym także w systemach sterowania i nawigacji robotów mobilnych [252]. Prowadząc działania na zbiorach ostrych, wykorzystuje się jednoznacznie określone operacje, takie jak przecięcie, suma, dopełnienie. Dla zbiorów rozmytych operacje takie są uogólnieniem swych odpowiedników dla zbiorów ostrych, jednak można je definiować na wiele sposobów, jeśli tylko spełniają aksjomaty właściwe dla operacji teoriomnogościowych [162]. Poprawnym operatorem negacji jest dowolna funkcja N : [0, 1] [0, 1] spełniająca warunek zachowania stałych (N(0) = 1, N(1) = 0), odwracania porządku oraz inwolucji. Operatorem przecięcia dla zbiorów rozmytych jest dowolna funkcja T : [0,1] 2 [0,1] spełniająca warunki przemienności, łączności i monotoniczności oraz tożsamości jedynki (dla e [0,1] T(e,1) = a). Odpowiednikiem sumy dla zbiorów rozmytych jest funkcja S : [0,1] 2 [0,1], która także musi spełniać warunki przemienności, łączności i monotoniczności oraz warunek tożsamości zera (dla e [0, 1] S(e, 0) = e). Istnieje wiele operatorów spełniających powyższe warunki [162]. Daje to teorii zbiorów rozmytych elastyczność niespotykaną w innych metodach przetwarzania informacji niepewnej, jednak wymaga wzmożonej uwagi i pewnego doświadczenia (wiedzy dziedzinowej) przy wyborze operatorów do konkretnej implementacji [46]. Twórca teorii zbiorów rozmytych, Zadeh, zaproponował funkcję minimum jako operator przecięcia i funkcję maksimum jako operator sumy zbiorów [47]: µ A (x) µ B (x) = min(µ A (x),µ B (x)), (3.39) µ A (x) µ B (x) = max(µ A (x),µ B (x)). (3.40) Innym operatorem przecięcia zbiorów rozmytych jest iloczyn algebraiczny: µ A (x) µ B (x) = µ A (x) µ B (x). (3.41) Spośród różnych operatorów sumy zbiorów rozmytych, używanych w rozważanym tu kontekście tworzenia mapy otoczenia robota, należy wymienić sumę algebraiczną: µ A (x) µ B (x) = µ A (x) + µ B (x) µ A (x) µ B (x) (3.42) 107

oraz operator wprowadzony przez Dombiego [162]: 1 µ A (x) µ B (x) = [ ( ) λ ( ) ] λ 1, (3.43) 1 1 + µ 1 A(x) + 1 µ 1 λ B(x) gdzie λ (0, ). Parametr λ pozwala modyfikować siłę operatora, który produkuje słabsze sumy zbiorów rozmytych dla mniejszych wartości λ. Podobne właściwości ma operator zdefiniowany przez Yagera [162]. Dopełnienie zbioru rozmytego definiowane jest jako µ Ā (x) = 1 µ A (x). (3.44) Do przetwarzania informacji niepewnej przydatny jest także operator koncentracji odpowiadający określeniu bardzo : µ A 2(x) = µ 2 A(x). (3.45) Metoda tworzenia mapy rastrowej Wykorzystanie zbiorów rozmytych do reprezentacji niepewności co do stanu komórek mapy rastrowej robota mobilnego wyposażonego w sonary zaproponowano w pracy [228]. Mapa rastrowa jest zbiorem komórek M[i,j], z których każda ma przypisany pewien stopień przynależności do zbioru komórek zajętych O i wolnych E w postaci pary funkcji przynależności µ O (i,j) i µ E (i,j). W teorii zbiorów rozmytych stopnie przynależności do zbiorów reprezentujących przeciwstawne hipotezy nie muszą się uzupełniać. W przypadku mapy rastrowej można to wyrazić jako µ E (i,j) 1 µ O (i,j). Własność ta pozwala reprezentować niewiedzę, a w szczególności odróżnić komórki mapy, o których nie ma wystarczającej informacji, od tych, dla których zebrano informacje sprzeczne. Komórki pustej mapy są inicjowane zerowymi wartościami funkcji przynależności µ O (i,j) i µ E (i,j), co odzwierciedla całkowity brak informacji. Sposób postępowania podczas aktualizowania wartości funkcji przynależności komórek jest następujący: 1. Dla każdej pozycji robota x k R = [x R y R θ R ] T, w której wykonywane są pomiary, oraz dla każdego pomiaru (pojedynczej wiązki sonaru lub skanera laserowego) r k i wykonanego w tej pozycji na podstawie modelu wiązki sensora wyznaczane są funkcje przynależności do zbiorów komórek zajętych O k i i komórek wolnych Ek i. 2. Zbiory utworzone w danej pozycji robota x k R są włączane (agregowane) do lokalnych zbiorów O k i E k : O k = Oi k, E k = Ei k. (3.46) i i 3. Globalne zbiory komórek zajętych O i komórek wolnych E aktualizowane są w wyniku agregacji istniejących już zbiorów globalnych ze zbiorami lokalnymi O k i E k : O = O O k, E = E E k. (3.47) 4. Wyznaczane są zbiory rozmyte opisujące komórki, których stan jest dwuznaczny (ang. ambiguous) A, czyli są one jednocześnie zajęte i wolne, oraz komórki, których stan pozostaje nieokreślony (ang. indeterminate) I, czyli nie są one ani zajęte, ani wolne: A = E O, I = Ē Ō. (3.48) 108

Istotnym elementem procedury tworzenia mapy rastrowej z wykorzystaniem zbiorów rozmytych jest dobór operatorów używanych w formułach (3.46) i (3.47). W przypadku wiązek pomiarowych, które nie nakładają się na siebie lub nakładają się w niewielkim stopniu (skanery laserowe), we wzorze (3.46) można zastosować prosty operator sumowania (3.40). Jeżeli wiązki się nakładają (sonary), to proponuje się agregację lokalną z wykorzystaniem operatora Dombiego [228]. Do agregacji lokalnych zbiorów komórek w zbiory globalne O i E może być zastosowany operator sumy lub uśredniania. Klasyczny operator Zadeha ma własność idempotencji X X = X, której nie mają takie operatory agregacji, jak suma algebraiczna (3.42) oraz operatory Dombiego (3.43) i Yagera [46]. Operator sumowania (ang. union) sprawdza się w środowiskach stacjonarnych lub quasi-stacjonarnych, ponieważ nigdy nie zmniejsza on wartości funkcji przynależności. W rezultacie stopień przynależności do zbioru komórek zajętych mapy rastrowej nigdy nie ulegnie zmniejszeniu, nawet jeżeli zajmujący dany obszar obiekt zmieni swoje położenie. Do operatorów mających tę własność należy suma algebraiczna wykorzystana w artykule [247] oraz operator Dombiego używany do aktualizacji mapy w pracy [228]. Użycie operatora, który nie jest idempotentny, może też prowadzić do nasycenia wartości funkcji przynależności danej komórki w wyniku niewielkiej liczby pomiarów. Z pomocą operatora Dombiego można jednak temu zapobiec, dobierając odpowiednie λ oraz parametry modelu wiązki sensora. Jeżeli otoczenie zmienia się w funkcji czasu, to operator zastosowany we wzorze (3.47) powinien zapewniać możliwość podwyższenia i obniżenia stopnia przynależności do danego zbioru komórek. Można to osiągnąć, stosując operator uśredniania, np. ważoną średnią arytmetyczną proponowaną w artykule [229] dla środowisk dynamicznych: a w (µ A (x),µ B (x)) = w 1µ A (x) + w 2 µ B (x), (3.49) 2 gdzie współczynniki w 1. Wadą operatora uśredniającego jest duża inercja do istotnej zmiany wartości funkcji przynależności dla danej komórki potrzebna jest duża liczba pomiarów wskazujących stan komórki niezgodny ze wskazaniami poprzednich pomiarów. Wszystkie operatory agregacji spełniają warunek przemienności, co umożliwia tworzenie mapy w sposób inkrementalny, a kolejność operacji (3.47) wykonywanych podczas aktualizacji nie ma znaczenia. Biorąc za podstawę zbiory komórek O, E, A oraz I, wyznaczone za pomocą wzorów (3.47) i (3.48), autorzy prac [228] i [229] zdefiniowali rozmyte zbiory komórek przydatnych do określonych funkcji systemu nawigacji robota mobilnego. Na przykład, jako bezpieczne dla ruchu robota M (ang. safe for motion) określone zostały komórki, które są bardzo puste, ale jednocześnie zostały zbadane, a uzyskana o nich informacja nie jest dwuznaczna: M = E 2 Ō Ā Ī. (3.50) Lingwistyczne określenie bardzo implementowane jest za pomocą operatora koncentracji (3.45). Powyższa definicja zbioru komórek bezpiecznych ilustruje elastyczność podejścia opartego na zbiorach rozmytych. Korzystając z właściwości niezależnej aktualizacji ewidencji hipotez occupied i empty (których funkcjonalnymi odpowiednikami są zbiory rozmyte O i E), można definiować reguły (predykaty) porównujące lub kontrastujące te informacje w sposób adekwatny do potrzeb danego zadania robota. 109

Modele sensorów Ponieważ stopnie przynależności danej komórki do zbiorów O i E nie muszą się uzupełniać, definiowane są odrębne modele wiązki, określające wartości funkcji przynależności do zbioru O i zbioru E. Struktura funkcji opisujących te modele jest identyczna ze strukturą modeli przedstawionych w podrozdziale 3.2.2 dla metody opartej na teorii Dempstera-Shafera. Można więc wykorzystać funkcje zdefiniowane w podrozdziale 3.2.2, które w metodzie zbiorów rozmytych będą reprezentować stopień przynależności każdej komórki do zbioru odpowiednio komórek zajętych i wolnych. Dla wiązki pomiarowej sonaru wartości funkcji przynależności są wyznaczane na podstawie funkcji (3.31), (3.32), (3.11) i (3.12) w następujący sposób: µ O (ρ,ϕ) = α o (ρ)β s (ϕ)γ s (ρ), (3.51) µ E (ρ,ϕ) = α e (ρ)β s (ϕ)γ s (ρ). (3.52) Dla wiązek pomiarowych skanerów laserowych wykorzystuje się funkcje dane wzorami (3.31), (3.32) i (3.16). Nie występuje funkcja obniżająca stopień przynależności komórki do zbiorów O i E wraz z rosnącą odległością: µ O (ρ,ϕ) = α o (ρ)β l (ϕ), (3.53) µ E (ρ,ϕ) = α e (ρ)β l (ϕ). (3.54) W przypadku użycia funkcji (3.31) i (3.32) do opisu niepewności w modelu wiązki opartym na zbiorach rozmytych należy dobrać stałe k o i k e w sposób uwzględniający stosowane operatory rozmyte. Jeżeli do tworzenia mapy globalnej przyjmuje się operator Dombiego lub inny o podobnych własnościach, to należy dobrać małe wartości k o i k e, by zapobiec nasyceniu funkcji przynależności. 3.2.4. Wyniki eksperymentów i porównanie metod Do testowania przedstawionych metod tworzenia map rastrowych posłużyły dane zebrane z sensorów robotów typu Labmate. Wykonano kilka eksperymentów, których celem była odpowiedź na pytania: Jak skutecznie opracowane modele sensorów przekształcają niepewność pomiaru w reprezentację właściwą dla danej metody? Jak na badanych rodzajach map jest reprezentowana informacja o wykrytych obiektach, mających różną wielkość i kształt i odmiennie postrzeganych przez poszczególne sensory? Czy są reprezentowane informacje sprzeczne? Jeżeli tak, to w jaki sposób. Jak badane metody działają w środowisku dynamicznym? W pierwszym eksperymencie badano skuteczność łączenia danych pochodzących z różnych sensorów: skanera LMS 200, skanera KARiI i sonarów w środowisku statycznym i przy nieruchomym robocie. Wykorzystując elementy dostępne w laboratorium (kartony i płyty styropianowe), zaaranżowano środowisko, które pokazano na rys. 3.5A. W punkcie 110

A Scena z ma³ym krzes³em B 210 cm Punkt umiejscowienia skanerów C D E Rysunek 3.5. Wizualizacja danych z sensorów i mapa rastrowa uzyskana metodą probabilistyczną (opis w tekście) oznaczonym okręgiem umieszczono robota, a w punkcie oznaczonym elipsą ustawiono krzesło obrotowe typowy element wyposażenia biurowego. Wykonano skany trzema rodzajami sensorów, których pomiary w różnym stopniu i w różny sposób odzwierciedlały geometrię przygotowanej sceny. Płaszczyzna pomiarów nisko umieszczonego skanera LMS 200 przypadała na wysokości podstawy ( nogi ) krzesła, znacznie większa część górna nie była dla tego sensora widoczna (rys. 3.5D). Umieszczony wyżej skaner KARiI postrzegał część oparcia krzesła, lecz widoczne dla niego zarysy kartonów miały inny kształt nie ustawiono ich równo, co zaznaczono linią przerywaną na rysunku 3.5A. Widoczne są też nieprawidłowe pomiary powstałe na skutek efektu śladu optycznego (rys. 3.5A i C). Sonary prawidłowo wykryły krawędź krzesła, lecz pochodząca z nich informacja była bardzo zgrubna w porównaniu z danymi uzyskanymi ze skanerów (rys. 3.5E). Mapę utworzoną metodą probabilistyczną na podstawie tych danych 1 przedstawiono na rys. 3.5B. Natomiast na rys. 3.6 zaprezentowano mapy utworzone z tych samych danych, ale metodą opartą na teorii Dempstera-Shafera, są to masy prawdopodobieństwa przypisane hipotezom occupied (rys. 3.6A), empty (rys. 3.6B) i unknown (rys. 3.6C). Na rysunku 3.7 przedstawiono wyniki uzyskane dla tych samych danych z zastosowaniem teorii zbiorów rozmytych. Są to zbiory komórek zajętych O (rys. 3.7A), wolnych E (rys. 3.7B), nieokreślonych I (rys. 3.7C) i niejednoznacznych A (rys. 3.7D). Zaprezentowano także wynik złożenia tych podstawowych typów informacji o stanie komórek, uzyskiwanych metodą rozmytą, w postaci mapy obszarów bezpiecznych dla ruchu (rys. 3.6E), utworzonej zgodnie ze wzorem (3.50) Porównując przedstawione mapy otoczenia, można dojść do następujących wniosków: 1 Współrzędne zaznaczone na krawędziach tej mapy oraz wszystkich pozostałych map rastrowych w tym rozdziale to indeksy komórek. 111

A B C Rysunek 3.6. Mapy rastrowe uzyskane na podstawie teorii Dempstera-Shafera (opis w tekście) Modele wiązek obu skanerów zaproponowane dla wszystkich metod skutecznie przekształcają niepewność zmierzonej odległości w stany komórek mapy. Duża szerokość wiązki sonaru powoduje powstawanie na mapach niepożądanych artefaktów. Sonary prawidłowo określają wolną przestrzeń, nie wnoszą jednak istotnej informacji dotyczącej położenia przeszkód. Wszystkie obiekty wchodzące w skład badanej sceny zostały zaobserwowane przez co najmniej jeden sensor. W metodzie probabilistycznej część tych informacji jest jednak tracona na mapie pokazanej na rys. 3.5B krzesło obrotowe jest odzwierciedlone jako zaledwie kilka punktów, brak też części przesuniętych kartonów. Podobnie wygląda mapa obszarów zajętych uzyskana metodą D-S (rys. 3.6A), jednak w tym przypadku przeciwstawne informacje dostarczone przez sensory znajdują się na mapie obszarów nieznanych (rys. 3.6C). Mapa komórek zajętych uzyskana metodą rozmytą zawiera informacje dostarczone przez wszystkie trzy sensory, a także artefakty spowodowane szerokością wiązki ultradźwiękowej. Na mapie komórek niejednoznacznych prawidłowo są zidentyfikowane wszystkie obszary, w których percepcja poszczególnych sensorów jest różna. A B C D E Rysunek 3.7. Mapy rastrowe uzyskane na podstawie teorii zbiorów rozmytych (opis w tekście) 112

W kolejnym eksperymencie wykorzystano podobną scenę, ustawioną z kartonów i płyt styropianowych. Jej szkic wraz z wymiarami i przykładowymi pomiarami użytych sensorów odległości przedstawiono na rys. 3.8A. Należy zwrócić uwagę, że umieszczony w środku obiekt znajdował się poniżej płaszczyzny skanowania skanera KARiI. W środowisku tym robot skanował otoczenie wszystkimi sensorami w 12 punktach (węzłach ścieżki). Informacja o położeniu i orientacji robota była dostarczana przez kamerę monitorującą umocowaną do stropu pomieszczenia (por. p. 2.5). Kolejno przedstawiono: mapę uzyskaną metodą probabilistyczną (rys. 3.8B), mapę obszarów zajętych otrzymaną metodą D-S (rys. 3.8C), zbiór komórek zajętych uzyskany metodą rozmytą (rys. 3.8D) oraz zbiór komórek nieokreślonych otrzymany tą samą metodą (rys. 3.8E). W wyniku integracji większej liczby pomiarów niż w poprzednim eksperymencie na mapie utworzonej metodą probabilistyczną zgromadzono wystarczająco wiele informacji świadczących o występowaniu obiektu niewidzialnego dla skanera KARiI, by pojawił się on na mapie. Mapa probabilistyczna jest jednak niespójna w dolnej części widoczny jest obszar wolny (wynik lustrzanych odbić ultradźwięków) oddzielony od głównej części mapy komórkami zajętymi. Taka sytuacja w rzeczywistości nie jest możliwa, ilustruje ona negatywne konsekwencje założenia o niezależności stanów sąsiednich komórek. Na mapie uzyskanej metodą D-S nie są uwzględnione obiekty, o których zebrano sprzeczne informacje. Wzór Dempstera używany do agregacji danych zawodzi w przypadku istotnej sprzeczności danych uzyskanych z sensorów. W opisywanym przypadku taka sprzeczność dotyczyła środkowego obiektu na mapie, gdyż jeden z sensorów konsekwentnie (we wszystkich pomiarach) wskazywał na jego nieobecność. W wyniku tego powstała masa prawdopodobieństwa wskazująca na hipotezę zawsze fałszywą, masa ta ze względu na normalizację we wzorze (3.22) była rozdzielana pomiędzy pozostałe hipotezy. Z tego powodu komórki mapy należące do obiektu ustawionego w środku sceny nie uzyskały odpowiedniej ewidencji wspierającej hipotezę occupied. O trudnościach w zastosowaniu teorii Dempstera-Shafera do tworzenia mapy rastrowej w przypadku silnie sprzecznych danych wspomniano w artykule [265], nie podano tam jednak konkretnego przykładu ilustrującego to zjawisko. Na potencjalne problemy wynikające z zastosowania sumy ortogonalnej (3.22) zwraca się także uwagę w rozważaniach teoretycznych dotyczących przetwarzania informacji niepewnej [47]. Mapa przedstawiająca rozmyty zbiór komórek zajętych jest najbardziej konserwatywna, gdyż zachowuje informacje dostarczone przez wszystkie sensory. Widoczny jest zarówno kompletny obrys sceny, jak i obiekt umieszczony w jej środku. Widać także, że zachowane zostały liczne artefakty, spowodowane fałszywymi pomiarami sonarów i błędami systematycznymi skanera KARiI. Obszary odpowiadające tym artefaktom zostały jednak w większości prawidłowo zidentyfikowane na mapie komórek niejednoznacznych. Na podstawie porównania map można stwierdzić, że metoda zbiorów rozmytych pozwala zachować najwięcej informacji na mapie. W przeciwieństwie do pozostałych metod w tej metodzie są łaczone tylko informacje tego samego rodzaju (komórki zajęte z zajętymi, wolne z wolnymi), natomiast agregacja różnych jakościowo danych i rozstrzyganie ewentualnych konfliktów odkładane jest do chwili tworzenia mapy o konkretnym przeznaczeniu. Pozwala to wykonywać te operacje w sposób zależny od kontekstu zadania. Tworzenie mapy oparte na teorii Dempstera-Shafera pozwala także operować funkcjami przekonania i wyobrażalności. Zastosowanie tych funkcji do budowy mapy rastrowej ilustruje rys. 3.9, na którym przedstawiono przykładowe mapy otoczenia otrzymane metodą D-S: mapę pomieszczenia laboratorium (rys. 3.9A i B) oraz korytarza w budynku Wydziału Elektrycznego Politechniki Poznańskiej (rys. 3.9C i D). Dane uzyskane ze skanera KARiI 113

sonary skaner KARiI (AMCW) Sick LMS-200 A wymiary [cm] B C D E Rysunek 3.8. Porównanie wyników badanych metod tworzenia mapy rastrowej dla trzech sensorów (opis w tekście) poddano korekcji błędów systematycznych pomiaru odległości i eliminacji pomiarów mało wiarygodnych, zgodnie z metodą opisaną w podrozdziale 2.4.2. Robot był pozycjonowany jedynie z użyciem odometrii. Widoczna jest różnica wartości funkcji przekonania (A, C) i wyobrażalności (B, D) w miejscach, gdzie zebrano sprzeczne dane z kolejnych pomiarów. Opisane wcześniej problemy z agregacją sprzecznych informacji w metodzie opartej na teorii D-S nie wystąpiły po zastosowaniu pojedynczego sensora dostarczającego wiarygodne dane. A C B D Rysunek 3.9. Wizualizacja funkcji przekonania i wyobrażalności dla map uzyskanych metodą D-S (opis w tekście) Przeprowadzono także eksperyment, którego celem było określenie właściwości badanych metod tworzenia mapy rastrowej w środowisku dynamicznym. Wykorzystano dane ze- 114

brane za pomocą skanera LMS 200 umieszczonego na robocie TRC Labmate w korytarzu przy Laboratorium Robotów Mobilnych IAiII. Położenie i orientację robota określono tylko za pomocą odometrii. Funkcję ruchomego obiektu pełniła osoba idąca przed robotem w polu widzenia skanera laserowego. Do utworzenia wszystkich map wykorzystano 17 skanów, których nie poddano wstępnej obróbce w celu korekcji błędów systematycznych i usunięcia punktów typu mixed pixel. Mapa utworzona metodą probabilistyczną jest najmniej użyteczna. Jedynie część punktów pochodzących od ruchomego obiektu została wyeliminowana (rys. 3.10A). Mapa powstała z użyciem teorii D-S nie zawiera artefaktów pochodzących od ruchomego obiektu (rys. 3.10B). Jednak obrys ścian nie jest odzwierciedlony prawidłowo. W wyniku błędów odometrii robota poszczególne skany się nie pokrywały. Spowodowało to błędne przypisanie na mapie D-S masy prawdopodobieństwa bycia pustym wielu komórkom reprezentującym ściany korytarza. Podobnie jak w poprzednich eksperymentach, mapa utworzona na podstawie teorii zbiorów rozmytych jest najbardziej konserwatywna, zachowuje wszystkie informacje. Na mapie komórek zajętych obrys ścian jest kompletny, jednak są wyraźnie widoczne punkty pochodzące od ruchomego obiektu (rys. 3.10C). Obszary odpowiadające położeniom obiektu ruchomego zostały prawidłowo zidentyfikowane na mapie komórek niejednoznacznych (rys. 3.10D). Pozwala to zbudować zarówno mapę, na której będzie uwzględnione położenie obiektu ruchomego, przydatną np. do planowania ruchu [229], jak i mapę uwzględniającą jedynie najbardziej wiarygodne pomiary, która może stanowić źródło informacji w procedurze samolokalizacji. A B C D Rysunek 3.10. Porównanie wyników badania metod tworzenia mapy rastrowej w środowisku dynamicznym W przedstawionych eksperymentach metody probabilistyczna i oparta na teorii Dempstera-Shafera dawały zbliżone wyniki, gdy dane z sensorów nie były sprzeczne. Gdy zebrane dane były sprzeczne, ujawniały się słabości obu metod. Trudności z podejściem bayesowskim mają źródło w jego założeniach (dopełnianie się prawdopodobieństw hipotez, niezależność pomiarów oraz stanu komórek), które w praktyce nie są spełnione. Założenia te ulegają pewnemu osłabieniu w podejściu opartym na teorii Dempstera-Shafera, w którym dopuszcza się hipotezę reprezentującą ignorancję, jednak użyta do aktualizacji wiedzy reguła Dempstera nie pozwala tej reprezentacji w pełni wykorzystać, gdyż rozstrzyga sytuacje konfliktowe, nie uwzględniając kontekstu, w jakim dana informacja jest dołączana. Podejście rozmyte do tworzenia mapy rastrowej wymaga najmniej założeń co do możliwego stanu komórek. Informacje są łączone w zbiorach reprezentujących dwie niezależne hipotezy. Pozwala to na elastyczne operowanie zgromadzonymi informacjami, jednak uzyskiwane mapy obszarów zajętych i wolnych są bardzo konserwatywne, a każda z nich rozpatrywana osobno 115

jest mało użyteczna. Wyniki agregacji na mapach rozmytych danych uzyskanych z sensorów istotnie zależą od wybranych operatorów podstawowych operacji występujących we wzorach (3.46) i (3.47). Zaprezentowane wyniki uzyskano, stosując operator (3.40) na etapie lokalnym i operator (3.43) do agregacji na mapie globalnej. Metoda bayesowska charakteryzuje się najmniejszą złożonością obliczeniową [247], jednak w praktyce czas tworzenia mapy z wykorzystaniem danych ze skanera laserowego jest zbliżony we wszystkich badanych metodach. Należy zauważyć, że w metodzie zbiorów rozmytych czas obliczeń zależy także od definicji operatorów. Najmniej zasobochłonna jest metoda probabilistyczna, która wymaga przechowywania jedynie wartości P(occ m) dla każdej komórki mapy. Metody oparte na teoriach D-S i zbiorów rozmytych wymagają odrębnej reprezentacji stanu zajętości i pustości dla każdej komórki. Ich zapotrzebowanie na pamięć jest więc dwa razy większe. Metoda rozmyta może wymagać bardziej rozbudowanej reprezentacji, jeżeli będą przechowywane wartości funkcji przynależności obliczone dla zbiorów I i A oraz ich pochodnych. Oprócz eksperymentów z tworzeniem map rastrowych, zaprezentowanych w niniejszym podrozdziale, wykonano także wiele innych, których wyniki przedstawiono w pracach [150, 151, 152, 282]. Różne metody aktualizacji mapy rastrowej porównano także w pracy [190]. Biorąc pod uwagę rezultaty tych badań oraz powyższą ich analizę, można stwierdzić, że w porównaniu z innymi metoda oparta na teorii zbiorów rozmytych pozwala na najlepszą reprezentację niepewnej informacji sensorycznej oraz umożliwia najbardziej elastyczne użycie tej informacji w systemie nawigacji robota mobilnego. 3.3. Mapa wektorowa 3.3.1. Podstawowe założenia i struktura mapy W rozprawie [271] zaproponowano reprezentację otoczenia robota w postaci mapy wektorowej składającej się z obiektów będących wielobokami lub łamanymi reprezentowanymi przez ciągi wektorów łączących ich naroża. Poszczególne odcinki są cechami reprezentującymi ściany pomieszczeń lub inne regularne struktury, charakteryzujące się przekrojem w przybliżeniu stałym w funkcji wysokości. Mapa wektorowa w proponowanej postaci pozwala na budowę modelu otoczenia statycznego wewnątrz budynków. W otoczeniu statycznym jedynym ruchomym obiektem jest robot. Zakres zastosowań mapy wektorowej można rozszerzyć na środowiska quasi-statyczne, w których pewne obiekty pojawiają się lub znikają w czasie działania robota. Sytuacja taka odpowiada przemieszczeniu obiektu (np. przeniesieniu krzesła z jednego miejsca w inne), jednak bez śledzenia jego ruchu [271]. W pracach [269, 270, 271] mapa była tworzona na podstawie danych dostarczonych przez skaner laserowy zamontowany na pokładzie robota mobilnego przy założeniu znajomości położenia i orientacji robota. Nie rozwiązywano więc problemu jednoczesnego tworzenia mapy i samolokalizacji. W praktyce podczas eksperymentów wykorzystywano dane z odometrii robota, co pozwalało jedynie na tworzenie map niewielkich wycinków otoczenia [270]. Zaproponowane w rozprawie [271] metody ekstrakcji cech 1 okazały się skuteczne i wykorzystano je w kolejnych wersjach programów tworzących mapy wektorowe, opisanych w pracach [264, 323]. W niniejszym rozdziale podano opis podstawowych metod ekstrakcji (wyodrębniania) cech z danych skanera laserowego i jednocześnie dokonano krótkiego porównania 1 W pracach [270, 271] nazywano je prymitywami geometrycznymi. 116

tych propozycji z innymi metodami znanymi z literatury. Bardziej złożone metody analizy danych uzyskanych z sensorów, w której wykorzystuje się dodatkowo lokalną reprezentację rastrową, przedstawiono w podrozdziale 3.4. Celem odtwarzania informacji geometrycznej z danych skanera laserowego jest uzyskanie cech geometrycznych wraz z opisem ich niepewności, które następnie będą zintegrowane w postaci mapy wektorowej z wykorzystaniem podejścia stochastycznego zaproponowanego przez Smitha i współpracowników [289, 290]. W przeciwieństwie do mapy proponowanej w rozprawie [271], opartej jedynie na analizie geometrii otoczenia, mapa stochastyczna uwzględnia niepewność wzajemnych relacji przestrzennych między robotem a cechami geometrycznymi oraz pomiędzy samymi cechami. Podejście to pozwala na jednoczesne tworzenie mapy i samolokalizację, co przedstawiono w rozdziale 4 niniejszej pracy. 3.3.2. Akwizycja i wstępne przetwarzanie danych ze skanera laserowego Krawędzie obiektów leżące w zasięgu skanera laserowego są wykrywane w danych dostarczonych w każdym cyklu pomiarów (cykl ten jest nazywany skanem). Dane ze skanera dostępne są w postaci odległości do wykrytego obiektu r oraz kąta ϕ, podawanych w układzie współrzędnych sensora (por. rys. 2.32 i 2.45). Położenie i orientacja skanera względem układu współrzędnych związanego z robotem dane są wektorem x S = [x s y s θ s ] T. W dalszych rozważaniach przyjmuje się, że wektor x S jest parametrem znanym, który nie wprowadza niepewności. Metody kalibracji parametrów zewnętrznych skanera laserowego, pozwalające ustalić położenie i orientację początku jego układu współrzędnych względem układu robota, przedstawiono między innymi w monografii [72]. Aktualne położenie i orientacja robota względem układu odniesienia związanego z otoczeniem (układu globalnego) dane są wektorem x R = [x R y R θ R ] T. Ponieważ w procedurach ekstrakcji cech geometrycznych wykorzystuje się reprezentację skanu w postaci tablicy punktów, dane są przekształcane do współrzędnych n p zeskanowanych punktów o współrzędnych kartezjańskich x p = [x y] T w lokalnym układzie współrzędnych związanym z robotem: x = r cos(ϕ + θ s ), y = r sin(ϕ + θ s ). (3.55) Podczas konwersji reprezentacji dokonywana jest też korekcja błędów systematycznych pomiaru odległości metodami opisanymi w podrozdziałach 2.4.2 i 2.4.3. W przypadku skanera KARiI procedura ta jest bardziej złożona i obejmuje odrzucanie pomiarów, których odchylenie standardowe jest nadmierne (wzór 2.68), co pozwala usunąć większość punktów powstałych na skutek efektu śladu optycznego. Rezultaty wstępnego przetwarzania danych ze skanera KARiI (rys. 3.11A) przedstawiono na rys. 3.11B. Niepewność położenia zeskanowanego punktu zależy od niepewności pomiaru. Modele niepewności pomiarów obu skanerów laserowych wykorzystywanych w przedstawionych tu badaniach opisano w podrozdziale 2.4. Oba urządzenia charakteryzują się niezależnością niepewności pomiaru odległości od niepewności związanej z ustawieniem zwierciadła odchylającego wiązkę. Oba rodzaje niepewności opisane są rozkładami normalnymi, co wyrażono formułami (2.73) i (2.78) dla skanera KARiI oraz (2.84) i (2.86) dla LMS 200. Biorąc powyższe pod uwagę, niepewność pomiaru m p = [r ϕ] T wyrażono macierzą diagonalną C m : [ ] σ 2 C m = r 0 0 σϕ 2. (3.56) 117

A B Rysunek 3.11. Wyniki korekcji błędów systematycznych w pomiarach skanerem KARiI Odchylenie standardowe σ r pomiaru odległości skanerem LMS 200 jest obliczane na podstawie wzoru (2.85), a w przypadku skanera KARiI wariancja σr 2 jest wyznaczana z próby. Wartości odchyleń standardowych kąta dla obu urządzeń są stałe, podano je w podrozdziale 2.4. Ponieważ zależność między wektorami m p i x p, wyrażona równaniem (3.55), jest nieliniowa, macierz kowariancji [ ] σ 2 C p = x σ xy σ yx σy 2, (3.57) reprezentująca niepewność położenia punktu, jest wyznaczana jako aproksymacja pierwszego rzędu: C p = J m C m J T m, (3.58) gdzie J m jest jakobianem transformacji (3.55) względem wektora m p : [ ] cos ϕ r sin ϕ J m =. (3.59) sin ϕ r cos ϕ 3.3.3. Grupowanie pomiarów Wnętrza budynków charakteryzują się wyraźnym rozdzieleniem poszczególnych obiektów w polu widzenia skanera. Dlatego pierwszym etapem przetwarzania skanu jest grupowanie otrzymanych punktów w celu ustalenia granicy między obiektami w otoczeniu robota. Grupowanie polega na podziale całego skanu traktowanego jako zbiór punktów na podzbiory reprezentujące odrębne obiekty. Jest to operacja istotna dla poprawności uzyskiwanej reprezentacji otoczenia, gdyż algorytmy segmentacji stosowane na kolejnych etapach przetwarzania skanu często łączą skupienia pomiarów odpowiadające odrębnym obiektom. Wynikiem działania algorytmów grupowania są grupy punktów odpowiadających poszczególnym obiektom. Usuwane są grupy, w których liczba pomiarów jest mniejsza niż założone minimum n pmin. Eliminuje to grupy, które nie nadają się do aproksymacji za pomocą odcinków. Podczas etapu grupowania eliminowane są też punkty odosobnione, które mogą wystąpić w skanie z powodu efektu śladu optycznego (por. rys. 3.14) lub dynamiki otoczenia (gdy obserwowane są ruchome obiekty). Grupowanie, będące pierwszą warstwą hierarchicznej struktury przetwarzania danych ze skanera, powoduje redukcję ilości danych i zwiększa stopień ich abstrakcji, działa też jako filtr usuwający część zakłóceń i artefaktów. Z literatury znane są różne podejścia do grupowania punktów w skanach 2D, wszystkie wykorzystują jednak informację o kolejności skanowania, tzn. znane uporządkowanie pomiarów w skanie. Najprostszy algorytm grupowania oparty jest na kryterium odległości eukli- 118

desowej d ei 1,i między dwoma kolejnymi punktami w zbiorze n p punktów o współrzędnych x pi = [x y] T, i = 1...n p, tworzących dany skan [119, 268]: d ei 1,i = (x i 1 x i ) 2 + (y i 1 y i ) 2 < d emax. (3.60) Przekroczenie określonego progu odległości d emax powoduje podział zbioru danych na dwie grupy. Ustalenie progu d emax w tej metodzie jest problematyczne, ponieważ odległość między kolejnymi punktami pomiarowymi jest proporcjonalna do odległości między skanerem a obiektem. Jest to szczególnie widoczne, gdy skanowana jest powierzchnia tworząca mały kąt z osią wiązki skanera. Jeżeli d emax będzie małe, to może dojść do rozdzielenia punktów należących do jednego obiektu, natomiast przy dużej wartości progu dojdzie do połączenia odrębnych obiektów. Kwestię ustalenia wartości progu można rozwiązać, badając stosunek odległości euklidesowych między kolejnymi punktami w skanie [271]. Poniżej przedstawiono algorytm 1 grupowania oparty na tej zasadzie. Wykorzystano w nim także bezwzględny próg odległości jako dodatkowe kryterium, w tym przypadku d emax może być duże (np. może odpowiadać szerokości typowego otworu drzwiowego). Algorytm 3.1 procedure POINTCLUSTERING(P,n p,k max,d emax ) 1 begin 2 n g:=1 ; {inicjacja licznika grup} 3 n g(x p1 ):=n g; n g(x p2 ):=n g; 4 for i:=1 to n p-2 do 5 begin 6 if d ei,i+1 /d ei+1,i+2 <k max and d ei+1,i+2 < d emax then n g(x pi+2 ):=n g 7 else 8 begin 9 n g:=n g+1; n g(x pi+2 ):=n g {początek nowej grupy} 10 end; 11 end; 12 return n g(p); {zwraca numery grup wszystkich punktów} 13 end. W powyższym algorytmie P jest zbiorem wszystkich punktów, k max to maksymalny dopuszczalny stosunek odległości między kolejnymi parami punktów, n g jest liczbą grup w skanie, a n g (x pi ) oznacza numer grupy i-tego punktu. Złożoność obliczeniowa algorytmu wynosi O(n) czas obliczeń zależy liniowo od liczby punktów w skanie. W metodzie tej nie jest jednak wykorzystywana informacja o niepewności pomiarów, a zastosowane kryterium odległości euklidesowej wymaga konwersji pomiarów na współrzędne kartezjańskie. Na rysunku 3.12A przedstawiono dane uzyskane ze skanera KARiI. Promienie okręgów reprezentują trzykrotne odchylenie standardowe zmierzonej odległości podanej jako surowe dane, natomiast na rys. 3.12B przedstawiono dane, w których błędy systematyczne pomiaru odległości skorygowano metodą opisaną wzorami (2.71) i (2.72) oraz usunięto punkty niespełniające kryterium (2.68) (zaznaczone kolorem szarym). Nie wszystkie pozostałe punkty nadają się jednak do dalszego przetwarzania. Wyniki użycia algorytmu 3.1 dla tych danych 1 Wszystkie algorytmy przedstawione w niniejszej pracy zostały zapisane w języku wzorowanym na Pidgin Algol zdefiniowanym w książce [7]. 119

przedstawiono na rys. 3.12C, natomiast na rys. 3.12D widoczne są tylko punkty zaakceptowane po wstępnym przetwarzaniu i grupowaniu, dla których zaznaczono elipsy niepewności położenia. A B C D Rysunek 3.12. Wyniki grupowania pomiarów skanerem KARiI za pomocą algorytmu 3.1 (opis w tekście) Metodę grupowania służącą do przetwarzania danych wyrażonych w naturalnych dla skanera współrzędnych biegunowych, w której wykorzystano rozszerzony filtr Kalmana (EKF), zaproponowano w pracy [72]. Opiera się ona na algorytmie śledzenia ruchomego obiektu [30]. W metodzie segmentacji ruch śledzonego obiektu odzwierciedla przemieszczanie się wiązki skanera laserowego wzdłuż linii prostej. Nieciągłość w obserwowanych obiektach lub gwałtowne zmiany orientacji płaszczyzn, po których przemieszcza się wiązka skanera, powodują zaburzenie śledzenia i wzrost błędu predykcji (innowacji filtru Kalmana). Powyższa metoda została zaimplementowana w pracy [323] i porównana eksperymentalnie z algorytmem 3.1. Wykorzystując dane ze skanera Sick LMS 200, uzyskano dla obu metod grupowania zbliżone jakościowo efekty podziału na grupy, przy czym metoda oparta na filtracji Kalmana wymaga znacznie większych nakładów obliczeniowych (pomimo braku konwersji współrzędnych), głównie ze względu na czasochłonność wyznaczania odległości Mahalanobisa dla każdego punktu skanu. Należy zaznaczyć, że dobór właściwych parametrów do metody grupowania EKF jest trudny i wymaga wielu eksperymentów [323], gdyż w przeciwieństwie do parametrów algorytmu opartego na odległości euklidesowej nie mają one wyraźnej interpretacji fizycznej. Metoda grupowania punktów powinna być szybka, skuteczna w różnych rodzajach otoczenia i wymagać jak najmniejszej liczby parametrów wprowadzanych przez użytkowni- 120

imax x Pi+1 r i+1 x Pi r i S Rysunek 3.13. Sposób określania maksymalnej odległości mierzonej w metodzie grupowania pomiarów ka. Biorąc te postulaty pod uwagę, opracowano algorytm grupowania działający na danych zapisanych we współrzędnych biegunowych. W celu ustalenia uniwersalnego progu podziału grup w algorytmie tym wykorzystuje się model niepewności pomiarów skanerem laserowym przedstawiony w podrozdziale 2.4. Zasadę działania tej metody przedstawiono na rys. 3.13. Kolejne promienie skanera laserowego, umieszczonego w punkcie S, padają na powierzchnię danego obiektu w punktach x pi i x pi+1, a odpowiadające im pomiary odległości oznaczono r i i r i+1. Pomiary wykonywane są dla kolejnych kątów ϕ i,ϕ i+1 = ϕ i + ϕ,...ϕ i+n = ϕ i + n ϕ, gdzie ϕ jest rozdzielczością kątową skanera. Skaner może dokonywać wiarygodnych pomiarów odległości jedynie wówczas, gdy kąt pomiędzy osią wiązki a normalną do obserwowanej powierzchni nie przekracza wartości granicznej φ imax, która została ustalona eksperymentalnie dla obu skanerów laserowych używanych w Laboratorium Robotów Mobilnych IAiII. Jeżeli i-ty odczyt wynosi r i, to maksymalną wartość następnego można ustalić, zakładając, że został on dokonany przy kącie padania wiązki φ imax. Rozpatrując trójkąt wyznaczony przez punkty S, x pi i x pi+1, można określić r i+1 na podstawie odległości r i oraz dwóch znanych kątów: ϕ i α = 90 φ imax : sin β r i+1max = r i sin α = r cos (φ imax ϕ) i. (3.61) cos φ imax W analogiczny sposób można wyznaczyć minimalną wartość kolejnego pomiaru: r i+1min = r i cos(φ imax + ϕ) cos φ imax. (3.62) Biorąc pod uwagę maksymalną i minimalną dopuszczalną wartość kolejnego pomiaru, dane wzorami (3.61) i (3.62), oraz niepewność pomiaru odległości wyrażoną dla rozpatrywanych skanerów laserowych formułami (2.73) i (2.84), algorytm grupowania pomiarów można wyrazić w następujący sposób: Algorytm 3.2 procedure RANGECLUSTERING(R,n p,φ imax, ϕ) 1 begin 2 k min:= cos(φ imax + ϕ cos φ imax ); {wsp. odległości minimalnej} 3 k max:= cos(φ imax ϕ cos φ imax ); {wsp. odległości maksymalnej} 4 n g:=1 ; {inicjacja licznika grup} 121

5 n g(r 1):=n g; 6 for i:=1 to n p-1 do 7 begin 8 if r i+1>(r ik min 3σ r) and r i+1<(r ik max + 3σ r) then n g(r i+1):=n g 9 else 10 begin 11 n g:=n g+1; n g(r i+1):=n g {początek nowej grupy} 12 end; 13 end; 14 return n g(r); {zwraca numery grup wszystkich pomiarów} 15 end. W powyższym algorytmie R jest zbiorem wszystkich pomiarów odległości. Algorytm działa bezpośrednio na odczytach odległości i jest prosty w implementacji. Złożoność obliczeniowa wynosi O(n), a dla każdego z n p 1 pomiarów wykonywane są tylko dwa mnożenia, dwa sumowania i dwie operacje porównania (linia 8). Ponieważ wartość n p dla danego sensora jest stała i znana, czas wykonania powyższej procedury także jest w przybliżeniu stały, co sprzyja implementacji w systemach czasu rzeczywistego. Uwzględniona jest niepewność pomiarów odległości, a metoda nie wymaga eksperymentalnego ustalania progów lub współczynników, gdyż są one oparte na parametrach użytego skanera. Metodę grupowania pomiarów, w której wykorzystano podobną zasadę wyznaczania progu odległości na podstawie określonego maksymalnego kąta obserwacji powierzchni, przedstawiono w artykule [54], jednak za pomocą tego algorytmu analizuje się odległości między kolejnymi punktami we współrzędnych kartezjańskich, a jego parametry nie są bezpośrednio oparte na charakterystyce sensora. Przykłady wykorzystania algorytmu 3.2 do grupowania pomiarów uzyskanych ze skanera Sick LMS 200 przedstawiono na rys. 3.14, na którym znakiem + zaznaczono pomiary odrzucone podczas grupowania. 3.3.4. Segmentacja W wyniku grupowania otrzymuje się zbiory punktów reprezentujące odrębne obiekty, które jednak nie zawsze mogą być przybliżone pojedynczymi odcinkami. Segmentacja pozwala wyodrębnić podgrupy punktów, dla których odcinki są wystarczająco dobrym przybliżeniem. Z literatury znanych jest wiele metod wykrywania cech geometrycznych wśród danych otrzymanych z sensorów odległości. Większość z nich stanowi adaptację algorytmów opracowanych wcześniej na potrzeby analizy obrazów z sensorów wizyjnych. Do najprostszych metod segmentacji należy sekwencyjne śledzenie krawędzi (ang. successive edge following), polegające na badaniu i porównywaniu wartości kolejnych pomiarów odległości w skanie. W najprostszej wersji metoda ta pozwala wykryć początek nowego odcinka, gdy różnica między kolejnymi zmierzonymi odległościami przekroczy zadany próg, i bywa używana do wstępnego grupowania (filtracji) pomiarów [226]. Wersja bardziej zaawansowana umożliwia wyszukiwanie różnych typów punktów końcowych segmentów (uskok, kąt, rampa) za pomocą specjalizowanych operatorów analizujących funkcję odległości r(ϕ) i jej pochodne [131]. Kompleksowa wersja powyższej metody (ang. total edge following) wymaga wstępnego grupowania danych. Wykorzystuje się w niej podobne operatory wykrywania nieciągłości, lecz liczba punktów analizowanych przez operator jest większa, co pozwala unikać minimów lokalnych funkcji odległości [164]. Zbliżony do metody sukcesywnego śle- 122

Rysunek 3.14. Wyniki grupowania pomiarów wykonanych skanerem LMS 200 za pomocą algorytmu 3.2 dzenia krawędzi jest przedstawiony przez Adamsa [4] algorytm, w którym rozszerzony filtr Kalmana wykorzystuje się do predykcji pomiarów. Metody służące do dopasowywania w sposób sekwencyjny linii prostej do zbioru punktów określa się mianem algorytmów śledzenia linii (ang. line tracking). W swej podstawowej odmianie działają one na danych zapisanych we współrzędnych kartezjańskich, nie wymagają jednak wstępnego grupowania punktów. W algorytmie śledzenia linii dokonuje się dopasowania prostej w sensie minimalnokwadratowym do przeanalizowanych już n punktów, a następnie sprawdza się odległość między tą prostą a kolejnym punktem x pn+1. Jeżeli jest ona mniejsza niż zadana wartość progowa, to punkt x pn+1 jest dodawany do zbioru punktów należących do budowanej prostej, a dopasowanie modelu prostej jest ponawiane. Przekroczenie wartości progowej powoduje natomiast rozpoczęcie budowy nowej prostej. Algorytm śledzenia linii zastosowano do segmentacji skanów między innymi w pracach [54, 316]. Metodą kompleksową działającą na danych wstępnie pogrupowanych we współrzędnych kartezjańskich jest algorytm iteracyjnego poszukiwania punktów końcowych (ang. Iterative 123

End Point Fit IEPF), przedstawiony w książce [95]. Za pomocą tego algorytmu dokonuje się rekursywnej aproksymacji grupy punktów zbiorem odcinków (łamaną). Prowadzona jest prosta pomocnicza między początkowym i końcowym punktem grupy, a następnie obliczana jest odległość pozostałych punktów od tej prostej. Jeśli wszystkie odległości leżą poniżej zadanego progu d min, algorytm kończy swoje działanie. W przeciwnym razie znajdowany jest punkt najbardziej odległy od prostej i w tym punkcie grupa jest dzielona na dwie części, dla których powyższe postępowanie jest powtarzane (rys. 3.15). Algorytm IEPF zbudowany jest zgodnie z zasadą dziel i zwyciężaj [7], a jego złożoność obliczeniowa wynosi O(n log n), gdzie n jest liczbą punktów w analizowanej grupie. Dobierając odległość d min, można regulować szczegółowość odwzorowania kształtu obiektów. Algorytm ten został zastosowany w wielu systemach służących do wyodrębniania odcinków z pomiarów sensorami odległości, zarówno sonarami [80] jak i skanerami laserowymi [54, 119, 251, 268, 271]. Podobny do IEPF algorytm d min Rysunek 3.15. Działanie algorytmu Iterative End Point Fit Iterative Line Fit (ILF) zastosowano do segmentacji grup punktów otrzymanych ze skanera [72]. Za pomocą tego algorytmu dokonuje się estymacji parametrów linii prostej z wykorzystaniem filtru Kalmana, a punkty końcowe odcinków wykrywa za pomocą testu odległości Mahalanobisa. Zaletą tego rozwiązania jest uwzględnienie niepewności położenia zeskanowanych punktów. Algorytmem IEPF jest także inspirowana metoda Split-and-Merge Fuzzy [54], łącząca koncepcję rekursywnego podziału grupy punktów z rozmytą analizą skupień. Do wykrywania linii prostych w danych pochodzących z sensorów odległości stosowana jest także transformata Hougha, dobrze znana jako metoda wykrywania cech geometrycznych (w ogólności dowolnych krzywych) na obrazach z kamer wizyjnych [95, 137]. W przeciwieństwie do wszystkich uprzednio przedstawionych metod transformata Hougha użyta do analizy pomiarów z sensora odległości nie wykorzystuje informacji o uporządkowaniu pomiarów w skanie. Jest to algorytm kompleksowy, działający na danych zapisanych we współrzędnych kartezjańskich, który nie wymaga wstępnego grupowania pomiarów. Zastosowany został między innymi w pracach [141, 226, 235]. Spotykane są także zupełnie inne podejścia do wyodrębniania odcinków, np. w artykule [251] opisano użycie iteracyjnego, niedeterministycznego algorytmu Expectation-Maximization (EM) do segmentacji zbioru skanów w trybie off-line. Zadowalające wyniki użycia algorytmu IEPF do przetwarzania danych symulowanych i rzeczywistych osiągnięto w rozprawie [271], a w pracy [323] porównano jego efektywność (jako elementu systemu SLAM) z metodą ILF. Na rysunku 3.16 porównano przykładowe mapy otoczenia otrzymane z zastosowaniem do segmentacji skanów algorytmu IEPF (rys. 3.16A) oraz algorytmu ILF (rys. 3.16B). W rezultacie do dalszego stosowania w niniejszej pracy wybrano algorytm Iterative End Point Fit jako podstawową metodę segmentacji skanów. Wadą algorytmu IEPF jest wrażliwość na położenie punktów, w których następuje podział grupy, będących końcowymi punktami znalezionych odcinków. Jeżeli punkty te są nadmier- 124

A B Rysunek 3.16. Porównanie wyników stosowania algorytmów IEPF (A) oraz ILF (B) do segmentacji skanów nie odległe od pozostałych należących do danej grupy, to mogą spowodować podział grupy na zbyt dużą liczbę odcinków i uzyskanie cech geometrycznych, które nie odzwierciedlają rzeczywistej struktury otoczenia (rys. 3.17A, B, C). Problem ten można rozwiązać, stosując podczas segmentacji zasadę dziel i łącz (ang. split-and-merge [137]). Polega ona na uzupełnieniu metody iteracyjnego poszukiwania punktów końcowych etapem łączenia (scalania) znalezionych segmentów. Po zakończeniu działania algorytmu IEPF na danej grupie punktów poszczególne segmenty są aproksymowane liniami prostymi (rys. 3.17D por. p. 3.3.5). Jeżeli sąsiadujące ze sobą segmenty są w przybliżeniu współliniowe, to będą scalone (rys. 3.17E), a równanie prostej będzie wyznaczone ponownie na podstawie wszystkich punktów należących do nowo utworzonego segmentu (rys. 3.17F). A B C D E F b³êdny podzia³ Rysunek 3.17. Ilustracja koncepcji segmentacji split-and-merge (opis w tekście) 125

3.3.5. Estymacja parametrów cech geometrycznych oraz ich niepewności Reprezentacja prostej wspierajacej W wyniku działania algorytmów segmentacji każda grupa uzyskana na poprzednim etapie przetwarzania skanu dzielona jest na fragmenty dogodne do opisu za pomocą odcinków. Dla każdego odcinka są wyznaczane współczynniki równania prostej, tzw. linii wspierającej (ang. support line), która zawiera ten odcinek. Linia wspierająca opisywana jest y linia wspieraj¹ca d i-1 d i d i+1 Rysunek 3.18. Reprezentacja linii wspierającej x wektorem parametrów, którego postać jest zdeterminowana wyborem matematycznej reprezentacji prostej. Najpopularniejsza, kierunkowa postać równania prostej y = ax + b nie pozwala na reprezentację wszystkich cech ze względu na występującą osobliwość. Problem ten można rozwiązać, stosując np. alternatywne postaci równania prostej dla odcinków bardziej poziomych i bardziej pionowych [27, 119, 316] lub wybierając równanie ogólne ax + by c = 0, pozwalające reprezentować dowolne proste [269]. W metodzie ekstrakcji cech wygodnie jest wyrazić równanie prostej w postaci normalnej: xcos φ + y sinφ ρ = 0, (3.63) gdzie φ to kąt nachylenia normalnej do danej prostej względem osi rzędnych (0 φ < 2π), a ρ (ρ > 0) jest odległością prostej od środka układu współrzędnych (rys. 3.18). Reprezentacja (3.63) umożliwia opis dowolnych prostych oraz pozwala na łatwe wyznaczanie odległości danego punktu od prostej. Reprezentacja normalna jest często używana w przetwarzaniu obrazu [137], została też wykorzystana w wielu systemach tworzenia mapy otoczenia lub samolokalizacji [14, 20, 22, 112, 141, 271]. Estymacja parametrów prostej Parametry równania prostej (3.63) x L = [ρ φ] T wyznacza się za pomocą metody najmniejszych kwadratów, minimalizuje się sumę kwadratów odległości euklidesowych punktów pomiarowych od prostej (rys. 3.18). W klasycznej postaci tej metody, znanej z prac dotyczących przetwarzania obrazów, nie uwzględnia się niepewności pomiarów [95] lub zakłada się identyczną niepewność wszystkich punktów branych pod uwagę [85]. Założenie to jest na ogół słuszne przy przetwarzaniu obrazu z kamery wizyjnej, jednak w pomiarach odległości skanerem laserowym nie jest spełnione. Z tego względu wykorzystano następującą ważoną funkcję kosztu: n p F(ρ,φ) = w i d 2 i = i=1 n p i=1 1 σ 2 r i (x i cos φ + y i sin φ ρ) 2, (3.64) 126

gdzie w i = 1 σ jest wagą zmniejszającą wpływ pomiarów o dużej wariancji mierzonej odległości na otrzymane parametry prostej. Niepewność kąta odchylenia wiązki nie jest wykorzy- r 2 i stywana jako waga, gdyż jest identyczna we wszystkich pomiarach. Dokonując minimalizacji równania (3.64) względem parametrów prostej F(ρ,φ) x L = 0, otrzymuje się następujące rozwiązanie: φ = 1 2 arctg 2 n p i=1 1 σ (ȳ r 2 σ y i )( x σ x i ) i np i=1 1, (3.65) σ [(ȳ r 2 σ y i ) 2 ( x σ x i ) 2 ] i ρ = x σ cos φ + ȳ σ sin φ, (3.66) gdzie x σ i ȳ σ są ważonymi średnimi współrzędnych n p punktów odcinka leżącego na danej prostej: n p n 1 1 1 p 1 x σ = np i=1 1 σ σ 2 x i, ȳ σ = np r 2 i i=1 r i i=1 1 σ σ 2 y i. (3.67) r 2 i i=1 r i W przeliczaniu wzoru (3.65) należy użyć funkcji arcus tangens dwuargumentowy (atan2), by otrzymać wartość kąta nachylenia (0 φ < 2π). Metodę ważonych najmniejszych kwadratów zastosowano także do estymacji parametrów prostej na podstawie danych ze skanera laserowego w pracy [54], gdzie nie sprecyzowano jednak postaci wagi, oraz w rozprawie [23]. Funkcja kosztu postaci (3.64) nie uwzględnia podczas estymacji parametrów prostej wszystkich składników niepewności położenia punktów pomiarowych. W pracy [235] zaproponowano sformułowanie dopasowania prostej do punktów otrzymanych ze skanera laserowego jako problemu nieliniowej estymacji metodą największej wiarygodności (ang. maximum likelihood) z uwzględnieniem także innych składników niepewności oprócz wariancji pomiaru odległości. Przedstawiona tam procedura estymacji parametrów prostej nie daje rozwiązania zamkniętego (wymaga iteracji), co ogranicza jej praktyczną przydatność. Istotnym czynnikiem wpływającym na prawidłowość i dokładność otrzymywanych linii wspierających są błędy nadmierne (ang. outliers). Większość punktów tego rodzaju w danych otrzymywanych ze skanera laserowego (np. efekty śladu optycznego) jest usuwana na wcześniejszych etapach wstępnego przetwarzania i grupowania, na etapie grupowania mogą się jednak pojawić punkty nieprawidłowo przypisane do danego segmentu przez algorytm IEPF, dotyczy to szczególnie skrajnych punktów segmentów. Metoda najmniejszych kwadratów zastosowana do estymacji parametrów modelu linii cechuje się dużą wrażliwością na błędy nadmierne. Wystąpienie wśród danych punktu znacznie oddalonego od grupy pomiarów prawidłowych, nazywanego punktem dźwigniowym (ang. leverage point), sprawia, że wyznaczone parametry przyjmują niewłaściwe wartości. Błędy tego rodzaju można wyeliminować, stosując odporne metody estymacji, w których wykorzystuje się do obliczeń jedynie część danych, a pomija się dane obarczone błędami nadmiernymi. Jedną ze znanych metod eliminacji błędów nadmiernych podczas estymacji parametrów cech jest algorytm RANSAC (ang. Random Sample Consensus) [108], stosowany w przetwarzaniu obrazu. Metoda ta stała się inspiracją przedstawionego poniżej algorytmu odpornej estymacji parametrów linii wspierającej. W algorytmie RANSAC ze zbioru m danych losowo są wybierane kolejne grupy k elementów, gdzie k jest minimalną liczbą elementów, na podstawie której można estymować poszukiwane parametry modelu. W przypadku estymacji 127

parametrów prostej k = 2. Na podstawie wylosowanych elementów znajdowane są parametry modelu, a następnie sprawdzane jest dopasowanie między tym modelem a pozostałymi danymi. Jeżeli model spełnia kryterium dopasowania (mały błąd resztowy) dla wystarczająco licznej grupy elementów, to grupa ta jest uznawana za zawierającą poprawne dane (ang. inliers) i algorytm kończy działanie. W przeciwnym razie losowanie jest powtarzane. Podstawowe różnice między RANSAC opisanym w artykule [108] a proponowanym algorytmem to założenie, że badana grupa punktów opisuje tylko jeden odcinek, oraz zastosowanie deterministycznego sposobu wyboru testowanych podzbiorów punktów. Algorytm działa na grupach punktów powstałych w wyniku segmentacji metodą IEPF, można się więc spodziewać, że punkty należą do jednego segmentu, w przeciwnym zaś razie są obarczone błędem nadmiernym. Ponieważ punkty są ustawione w kolejności skanowania, można je przeglądać systematycznie, poczynając od obu końców listy danych i tworząc pary coraz mniej odległych punktów. Algorytm 3.3 procedure ROBUSTLINEFIT(P,n p,n pmin,d fit ) 1 begin 2 done:=false; {inicjacja} -1 do 4 for n r:=n p-1 downto np 2 3 for n l :=2 to np 2 +1 do 5 begin 6 [ρ φ] T :=LINEFIT(x pnl,x pnr ); {estymacja parametrów prostej} 7 I:= ; score:=0; {inicjacja zbioru punktów i licznika} 8 for i:=1 to n p do if DISTANCE(x pi,ρ,φ) <= d fit then 9 begin 10 I:=I+x pi ; score:=score+1; {dołączenie i-tego punktu do zbioru} 11 if score >= n pmin then 12 begin 13 done:=true; break {znaleziono dopasowanie, wyjście z pętli} 14 end; 15 end; 16 end; {koniec głównej pętli} 17 if done then [ρ φ] T :=LINEFIT(I) {estymacja parametrów prostej} 18 else fail; {nie znaleziono dopasowania o zadanych parametrach} 19 end. W powyższym algorytmie P jest zbiorem wszystkich punktów zaliczonych do grupy podczas segmentacji, a I jest zbiorem zaakceptowanych punktów, należących do segmentu (inliers). Procedura LINEFIT (linie 6 i 17) jest realizacją metody ważonych najmniejszych kwadratów, zgodnie ze wzorami (3.65 3.67). Wyznaczenie odległości punktu od prostej (linia 8) polega na wyliczeniu wartości równania (3.63) dla danego punktu. Podczas przeglądania danych pomijane są punkty skrajne (linie 2 i 3), gdyż właśnie one są najczęściej obarczone błędem nadmiernym w wyniku niedokładnej segmentacji. Metoda wymaga podania dwóch parametrów: dopuszczalnej odległości punktu od prostej d fit oraz minimalnej liczby poprawnych danych w grupie n pmin. Na podstawie modelu niepewności dla pomiaru odległości można przyjąć d fit = 3σ r, natomiast n pmin należy ustalić eksperymentalnie, w zależności od spodziewanej (typowej) ilości błędów nadmiernych w danych ze skanera. Podczas eksperymentów z danymi ze skanera LMS 200 przyjmowano n pmin od 75% do 95% n p (liczby punktów w grupie), przy czym nie obserwowano istotnej zmiany wartości estymowanych parametrów prostej. W algorytmie 3.3 decydujący wpływ na czas obliczeń ma test 128

odległości punktu od prostej (linia 8), który wykonywany jest n p razy dla każdej testowanej pary punktów. Maksymalna liczba prób wynosi 1 4 n2 p 2n p + 4. Ponieważ n p określa wielkość zbioru danych wejściowych, pesymistyczne oszacowanie złożoności obliczeniowej tego algorytmu wynosi O(n 3 ). Czas wykonania proponowanej procedury odpornej estymacji podczas prób na zbiorach o rozmiarach od 20 do 200 punktów (wśród których ilość błędów nadmiernych była nie większa niż 20%) nie przekraczał dwukrotnego czasu wykonania standardowej procedury estymacji metodą najmniejszych kwadratów. A B C D Rysunek 3.19. Porównanie wyników estymacji równania linii wspierającej metodą najmniejszych kwadratów (linia przerywana) i za pomocą algorytmu 3.3 (linia ciągła) Na rysunku 3.19 porównano wyniki estymacji parametrów linii wspierającej metodą najmniejszych kwadratów (linia przerywana) i za pomocą algorytmu 3.3 (linia ciągła). Punkty oznaczają zaakceptowane dane, a okręgi dane uznane za obarczone błędem nadmiernym. Przedstawiono wyniki dla dwóch grup po 20 punktów (rys. 3.19A, B) i grupy 200 punktów (rys. 3.19C) zawierających punkty dźwigniowe oraz przykład obrazujący odporność metody na typowe błędy powstające podczas segmentacji metodą IEPF (rys. 3.19D). Jeżeli wykorzystywane są pomiary odległości obarczone znacznymi błędami przypadkowymi lub dostępny model sensora nie jest wystarczająco precyzyjny, to ustalenie wartości d fit, która zapewnia wystarczającą dokładność estymacji prostych, a jednocześnie nie powoduje odrzucania zbyt wielu grup punktów, może być trudne. Można wówczas rozważyć zastosowanie poniższego algorytmu, pozwalającego na adaptację szerokości przedziału tolerancji wokół prostej kosztem wydłużenia czasu obliczeń. 129

Algorytm 3.4 procedure ROBUSTLINEFIT2(P,n p,n pmin,d min fit,d max fit ) 1 begin 2 done:=false; d fit :=d min fit ; { inicjacja } 3 while not done and d fit < d max fit do 4 begin 5 ROBUSTLINEFIT(P,n p,n pmin,d fit ); {wywołanie głównej pętli alg. 3.3} 6 d fit :=d fit + d fit {rozszerzenie strefy tolerancji} 7 end; 8 if done then [ρ φ] T :=LINEFIT(I) {estymacja parametrów prostej} 9 else fail; {nie znaleziono dopasowania o zadanych parametrach} 10 end. W algorytmie tym wykorzystano podstawową część (linia 5) algorytmu 3.3, która wywoływana jest wielokrotnie dla kolejno zwiększanych odległości d fit. Parametrami wejściowymi są d min fit i d max fit, odpowiednio minimalna i maksymalna szerokość przedziału tolerancji wokół prostej, w którym mogą się znajdować dane. Złożoność obliczeniowa szacowana jest na O(kn 3 ), gdzie k = dmax fit d min fit d fit. Wyznaczanie macierzy kowariancji Gdy za pomocą jednego z powyższych algorytmów zostanie określony zbiór I punktów należących do aproksymowanego segmentu i ustalone zostaną parametry równania (3.63), należy wyznaczyć niepewność wektora x l = [ρ φ] T w postaci macierzy kowariancji: [ ] σ 2 C l = ρ σ ρφ σ φρ σφ 2. (3.68) Macierz kowariancji (wzór 3.68) powstaje przez propagację niepewności pozycji C p (3.57) punktów należących do zbioru I. Wektor x l = [ρ φ] T otrzymuje się w wyniku minimalizacji sumy błędów resztowych danej równaniem (3.64). Do wyznaczenia macierzy C l można wykorzystać metodę Haralicka [124], jak uczyniono to w pracy [54]. Ponieważ jednak funkcja wiążąca zmienne x l i x p dana jest w postaci jawnej równaniami (3.65) i (3.66), macierz (3.68) można wyznaczyć za pomocą bezpośredniej propagacji niepewności z użyciem równania postaci (3.1): C l = J p C P J T p, (3.69) gdzie C P jest blokowo diagonalną macierzą o wymiarach 2n p 2n p, złożoną z macierzy kowariancji (3.57) wszystkich punktów wykorzystanych podczas estymacji wektora x l : σx 2 1 σ x1y 1 σ y1x 1 σ 2 0... 0 y 1 σ x 0 2 2 σ x2y 2 σ C P = y2x 2 σ 2... 0 y 2........ σx 2 0 0... np σ xnp y np σ ynp x np σy 2 np a J p jest jakobianem o wymiarach 2 2n p i następującej postaci:, (3.70) 130

J p = [ ρ x 1 φ x 1 ρ y 1 φ y 1 ρ x 2 φ x 2 ρ y 2... φ y 2... ρ x np φ x np ρ y np φ y np ]. (3.71) Dla zwięzłości zapisu i zachowania czytelności składowe jakobianu (3.71) podano bez uwzględnienia wag oraz wprowadzono oznaczenia: n p [ L = 2 (ȳ y i )( x x i ), M = (ȳ yi ) 2 ( x x i ) 2], (3.72) i=1 które pozwalają zapisać zależności (3.65) i (3.66) jako: φ = 1 ( ) L 2 arctg, ρ = xcos φ + ȳ sinφ. (3.73) M Po uwzględnieniu powyższych oznaczeń składowe jakobianu (3.71) dane są wyrażeniami: n p i=1 φ x i = L x i M M x i L 2(L 2 + M 2 ), L = 2ȳ + y i, x i n M p = 6 x k + 2 x, (3.74) x i k=1 φ y i = L y i M M y i L 2(L 2 + M 2 ), L = 2 x + x i, y i n M p = 2 y k + 2ȳ, (3.75) y i k=1 ρ = ȳ cos φ φ xsin φ φ + 1 cos φ, (3.76) x i x i x i n ρ = ȳ cos φ φ xsinφ φ + 1 sinφ. (3.77) y i y i y i n W przeliczaniu wzorów (3.73) należy użyć funkcji arcus tangens dwuargumentowy (atan2). φ W wyrażeniach (3.76) i (3.77) wykorzystywane są wartości x i i φ y i obliczone uprzednio, odpowiednio ze wzorów (3.74) i (3.75). Podobne podejście do określania niepewności parametrów linii wspierającej przedstawiono w rozprawie [23], gdzie równanie prostej estymowane jest na podstawie danych zapisanych we współrzędnych biegunowych, w związku z czym propagacja niepewności następuje bezpośrednio z macierzy (3.56). Uwzględniony został jednak tylko składnik niepewności związany z pomiarem odległości. Zbliżoną metodę zastosowano także w pracy [141]. Pierwotnie zaproponowano ją do wyznaczania niepewności prostych na obrazach [85], przy czym założono identyczną niepewność wszystkich punktów należących do prostej, które to założenie w przypadku danych ze skanera laserowego nie jest spełnione. Ustalanie parametrów odcinków Cechami na mapie wektorowej są odcinki, a równanie linii wspierającej danego odcinka opisuje nieskończoną prostą. By otrzymać odcinek opisujący krawędź obiektu, należy wyznaczyć jego początek oraz koniec. Punkty opisujące początek x 1,y 1 oraz koniec x 2,y 2 odcinka powstają przez rzutowanie odpowiednio pierwszego i ostatniego punktu otrzymanego z pomiarów i należącego do zbioru I na linię wspierającą danego odcinka [270, 271]. Istnieje wiele możliwości reprezentacji odcinka na podstawie jego linii wspierającej wyrażonej równaniem (3.63). Jedną z nich jest wyznaczenie środka odcinka i jego długości. 131

Reprezentacja ta jest bliska minimalnej pod względem liczby parametrów, a jednocześnie zapewnia efektywność przekształceń geometrycznych cech. Środek odcinka opisany jest współrzędnymi p f = [x f y f ] T, a długość skalarnym parametrem l f. Obie te wielkości są obliczane na podstawie wcześniej ustalonych współrzędnych końców odcinka. Nie jest określana niepewność położenia środka odcinka ani niepewność jego długości. Te parametry odcinka zależą od położenia jego punktów końcowych. Niepewność tego położenia zależy z kolei nie tylko od niepewności pomiarów, lecz także od parametrów algorytmów segmentacji (d min ) i estymacji równania linii wspierającej. W związku z tym, w przeciwieństwie do określania niepewności parametrów wektora x l, nie można w sposób wiarygodny przeprowadzić propagacji niepewności od pomiarów do parametrów odcinka. Podobne wnioski dotyczące niepewności długości odcinków odtwarzanych z obrazów sformułowano w artykule [330], małą wiarygodność długości odcinka jako parametru opisującego krawędzie znajdowane na obrazach z kamer wizyjnych wykazano także w pracy [192]. 3.3.6. Tworzenie mapy globalnej przy znanej pozycji robota Cechy geometryczne otrzymane w wyniku przetwarzania pojedynczego skanu tworzą mapę lokalną, na której wszystkie współrzędne wyrażone są w układzie współrzędnych robota. Połączenie zestawów cech otrzymanych w różnych pozycjach robota pozwala na utworzenie mapy globalnej, na której cechy te są określone względem danego układu odniesienia (układu globalnego), zewnętrznego względem robota. Schemat blokowy na rys. 3.20 obrazuje przepływ informacji w procedurze tworzenia mapy wektorowej otoczenia przy założeniu znajomości pozycji robota w chwili wykonywania każdego skanu. Pozycja globalna Stan robota Cechy w uk³adzie globalnym Obserwacje (mapa lokalna) Dopasowanie cech NIE TAK Nowe cechy Aktualizacja cech Mapa globalna Rysunek 3.20. Schemat blokowy procedury tworzenia globalnej mapy wektorowej W fazie tworzenia mapy globalnej współrzędne odcinków oraz parametry linii wspierających muszą być wyrażone w globalnym układzie współrzędnych. Konieczna jest do tego znajomość estymaty położenia i orientacji robota x R w chwili wykonywania skanu będącego podstawą nowej mapy lokalnej: x lg = g(x l,x R ). (3.78) Wektor x R = [x R y R θ R ] T może pochodzić z niezależnego systemu pozycjonowania robota (np. kamery monitorującej, por. p. 2.5). 132

Elementy wektora opisującego prostą w układzie globalnym x lg = [ρ g φ g ] T wyznacza się z zależności: ρ g = ρ + x R cos(φ + θ R ) + y R sin(φ + θ R ), φ g = φ + θ R, (3.79) a położenie środka odcinka w układzie globalnym oblicza się w następujący sposób: x fg = x R + x f cos θ R y f sin θ R, y fg = y R + x f sinθ R + y f cos θ R. (3.80) Niepewność pozycji prostej na mapie globalnej C lg zależy od niepewności jej pozycji na mapie lokalnej C l (3.68) oraz niepewności pozycji robota podczas tworzenia danej mapy lokalnej C R : C lg = J l C l J T l + J r C R J T r, (3.81) gdzie J l,j r są jakobianami przekształcenia (3.79) względem wektorów, odpowiednio x l oraz x R : [ ] 1 xr sin(φ + θ J l = R ) + y R cos(φ + θ R ), (3.82) 0 1 [ ] cos(φ + θr ) sin(φ + θ J r = R ) x R sin(φ + θ R ) + y R cos(φ + θ R ). (3.83) 0 0 1 Globalna mapa wektorowa, podobnie jak mapa rastrowa, powstaje przez łączenie informacji z kolejnych map lokalnych. Fundamentalna różnica wynika ze sposobu traktowania niepewności co do przedmiotu pomiaru. Niepewnością tego rodzaju w różnym stopniu są obarczone pomiary wykonywane przez wszystkie sensory zewnętrzne używane w robotach mobilnych (por. p. 2.1). Podczas tworzenia mapy rastrowej (niezależnie od użytego formalizmu matematycznego) niepewność co do przedmiotu pomiaru jest uwzględniana implicite przez przypisanie porcji danych (ewidencji) do określonej komórki mapy. Na etapie tworzenia mapy globalnej ten rodzaj niepewności nie jest odrębnie uwzględniany. Podczas tworzenia mapy wektorowej niepewność położenia cech geometrycznych określana jest na podstawie statystyki związanej z pomiarami i metodami ich przetwarzania. W wynikowych macierzach kowariancji nie uwzględnia się niepewności co do przedmiotu pomiaru, która musi być uwzględniona explicite na etapie tworzenia mapy globalnej, gdy podejmowane są decyzje, które cechy z mapy lokalnej i globalnej będą połączone. Decyzje te są podejmowane na podstawie dopasowania cech (ang. feature matching). Proces dopasowywania cech geometrycznych składa się z testu oceniającego stopień dopasowania linii wspierających obu cech: globalnej x lg i lokalnej x l oraz testu pozwalającego na sprawdzenie, czy rozważane cechy mogą być częściami jednego odcinka. Linie wspierające dopasowuje się z uwzględnieniem niepewności ich parametrów ρ i φ. Kryterium dopasowania jest kwadrat odległości Mahalanobisa: d 2 (x l,x lg ) = ( (x l x lg ) T (C lg + C l ) 1 (x l x lg ) ), (3.84) gdzie x lg i x l są wektorami parametrów linii wspierających dopasowywanych cech, a C lg i C l są macierzami kowariancji tych parametrów. Jeżeli x jest n-wymiarowym wektorem zmiennych losowych o rozkładzie normalnym, x jego wartością oczekiwaną, a C macierzą kowariancji, to skalarna wielkość dana równaniem: d 2 = (x x) T C 1 (x x) (3.85) 133

jest odległością od x znormalizowaną macierzą kowariancji. Odległość ta wyznacza elipsoidę 1 o środku w x. Zmienna losowa zdefiniowana formą kwadratową (3.85) ma rozkład χ 2 [31]. Odległość Mahalanobisa dana równaniem (3.84) określa podobieństwo dwóch cech. Im odległość ta jest mniejsza, tym cechy są bardziej podobne [31, 166]. Wartość progowa w teście podobieństwa dwóch cech jest dobierana z tablic rozkładu χ 2 dla r stopni swobody i określonego prawdopodobieństwa α podjęcia właściwej decyzji: d 2 (x l,x lg ) < χ 2 r,α. (3.86) Dla wektora x l o dwóch stopniach swobody i 95-procentowym prawdopodobieństwie akceptacji pasujących cech próg ten jest równy 5,99. a Test oparty na odległości n test 1. test 2. m Rysunek 3.21. Geometryczne testy odległości między odcinkami x b c d y Mahalanobisa pozwala ocenić podobieństwo parametrów badanych cech w sposób uwzględniający probabilistyczną naturę tych parametrów. Użyte w tym teście parametry opisują jednak tylko linie wspierające odcinków. W wielu przypadkach całkowicie odrębne obiekty w otoczeniu są reprezentowane przez odcinki współliniowe. Rozróżnienie takich cech, które nie mogą być połączone na mapie globalnej, jedynie na podstawie parametrów nieskończonych prostych jest niemożliwe. W związku z tym wykonuje się testy podobieństwa uwzględniające położenie i długość odcinka, który jest reprezentowany przez daną cechę. Ponieważ parametrom x p i l f nie towarzyszy opis niepewności, kryteria podobieństwa odcinków są oparte na relacjach geometrycznych i odległościach euklidesowych. Wykonuje się dwa testy odległości między końcami odcinków (oznaczenia jak na rys. 3.21). 1. Pierwszy jest test bliskości odcinków. Wynik jest pozytywny, jeżeli suma odległości końców krótszego odcinka od odcinka dłuższego jest mniejsza od wartości progowej: m + n < d 1. 2. Jeżeli wynik poprzedniego testu jest pozytywny, to sprawdzany jest warunek zachodzenia na siebie badanych odcinków. Wynik tego testu jest pozytywny, gdy spełniona jest zależność: a + b + c + d < (2x + 2y + d 2 ). Wartości progowe występujące w powyższych warunkach są dobierane eksperymentalnie w zależności od typu otoczenia i parametrów metody segmentacji (algorytmu IEPF). Podczas dopasowywania cech powyższe testy geometryczne są wykonywane przed testem odległości Mahalanobisa, który jest znacznie bardziej czasochłonny. Pozwala to ograniczyć liczbę cech, dla których badana jest odległość Mahalanobisa, gdyż większość potencjalnych par cech jest odrzucana ze względu na brak dopasowania geometrycznego. Cechy, które zostały dopasowane na podstawie powyższych testów, są łączone. Polega to na modyfikacji parametrów odcinków zawartych na mapie globalnej na podstawie parametrów nowych odcinków z aktualnej mapy lokalnej. Jeżeli nowego odcinka dodawanego z mapy lokalnej nie można dopasować do żadnego istniejącego już odcinka, to jest on bezpośrednio zapisywany na mapie globalnej bez etapu łączenia. 1 Dla n = 2 jest to elipsa. 134

Procedura łączenia (fuzji) umożliwia estymację parametrów modyfikowanego odcinka na podstawie parametrów nowego odcinka i odcinka, do którego nowy odcinek został dopasowany, z uwzględnieniem niepewności. Zaktualizowane parametry równania (3.63) są obliczane na podstawie wektorów x lg i x l oraz macierzy kowariancji C lg,c l. Ponieważ zgodnie z założeniami mapa globalna nie zawiera elementów dynamicznych, do wyznaczania parametrów cech geometrycznych zastosowano statyczny filtr Kalmana [289]: K = C lg (C lg + C l ) 1, (3.87) x lgn = x lg K(x lg x l ), (3.88) C lgn = (I K)C lg, (3.89) gdzie x lgn jest wektorem parametrów zaktualizowanej prostej, C lgn jego macierzą kowariancji wyrażającą niepewność, a K wzmocnieniem filtru Kalmana (macierzą wag). Jako końce zaktualizowanego odcinka przyjmuje się te punkty końcowe odcinków składowych, które leżą najdalej od siebie względem nowej linii odniesienia i czynią zaktualizowany odcinek najdłuższym. Powyższe procedury umożliwiają jedynie określenie niepewności położenia cechy w przestrzeni. Dodatkowo każdy odcinek na mapie globalnej ma parametr n o oznaczający liczbę odcinków lokalnych, z których został złożony. Parametr ten jest inkrementowany, jeżeli do danego odcinka w mapie globalnej został dopasowany odcinek z kolejnej mapy lokalnej [271]. 3.4. Modele hybrydowe i konwersja reprezentacji 3.4.1. Reprezentacja hybrydowa w przetwarzaniu danych z sensorów odległości W poprzednich podrozdziałach przedstawiono metody przetwarzania danych uzyskanych z sensorów odległości (przede wszystkim skanów laserowych), wykorzystywane do tworzenia rastrowych i wektorowych map otoczenia autonomicznego robota mobilnego. Wyniki eksperymentów przedstawione w podrozdziale 3.2 dowiodły, że mapy rastrowe stanowią skuteczne narzędzie integracji i filtracji danych uzyskanych z sensorów odległości. Mapa rastrowa pozwala akumulować dane zebrane w kilku pozycjach robota, eliminować większość nieprawidłowych pomiarów i potwierdzać wiarygodność pozostałych. Jednak z punktu widzenia potrzeb całego systemu nawigacji robota mapa wektorowa stanowi bardziej syntetyczną i efektywną reprezentację informacji przestrzennej. Sposobem konwersji reprezentacji i wyodrębiania użytecznej informacji jest ekstrakcja cech geometrycznych z mapy rastrowej i umieszczanie ich na mapie wektorowej. Zaletą reprezentacji wektorowej odtworzonej z mapy rastrowej jest mniejsza liczba cech wygenerowanych na podstawie błędnych danych z sensorów w porównaniu z bezpośrednim wyodrębnieniem tych cech z serii pomiarów odległości. Jest to następstwem zgromadzenia większej ilości informacji na mapie rastrowej niż w pojedynczej serii pomiarów skanera lub zestawu sonarów. Unika się w ten sposób konieczności określania parametrów cech geometrycznych na podstawie niewielkiej ilości danych i/lub danych pochodzących z pojedynczej pozycji robota. Jest to szczególnie istotne w przypadku korzystania z pomiarów sensorów ultradźwiękowych, gdyż niepewność co do przedmiotu pomiaru, zależna od konfiguracji tych sensorów względem otoczenia, jest tu składnikiem dominującym. 135

W niniejszym podrozdziale zaprezentowano metody przetwarzania i gromadzenia danych z sensorów, które wykorzystują oba typy reprezentacji: rastrową i wektorową w celu uzyskania reprezentacji hybrydowej, łączącej zalety mapy rastrowej i wektorowej. Docelowa reprezentacja otoczenia ma postać wektorową i składa się z cech geometrycznych w postaci odcinków zdefiniowanych w podrozdziale 3.3. Mapa rastrowa używana jest jako pośrednia i pomocnicza reprezentacja lokalna lub globalna na etapie ekstrakcji cech z danych. 3.4.2. Mapa rastrowa jako reprezentacja pośrednia Ponieważ docelowa reprezentacja informacji przestrzennej ma postać mapy wektorowej, korzystne jest zastosowanie metody tworzenia pośredniej mapy rastrowej, pozwalającej na identyfikację (wyróżnienie) komórek zawierających ewidencję wspierającą hipotezę o istnieniu w tym miejscu cechy geometrycznej poszukiwanego typu w tym przypadku odcinka. Spośród badanych w podrozdzia- Rysunek 3.22. Wizualizacja przynależności do zbioru komórek wspierających le 3.2 sposobów tworzenia mapy rastrowej metoda oparta na formalizmie zbiorów rozmytych umożliwia najelastyczniejsze operowanie zgromadzonymi informacjami niepewnymi przez definiowanie zbiorów rozmytych o właściwościach adekwatnych do potrzeb aktualnego zadania robota. Korzystając z tej właściwości metody zbiorów rozmytych, zdefiniowano zbiór komórek wspierających (ang. support cells), które są jednocześnie bardzo zajęte, jednoznaczne i określone. Określenie bardzo, podkreślające różnicę między małym i dużym stopniem przynależności do zbioru komórek zajętych, jest wyrażane za pomocą operatora koncentracji: S = O 2 Ā Ī. (3.90) W proponowanej jako reprezentacja pośrednia mapie rastrowej FSG (ang. Fuzzy Support Grid), tworzonej za pomocą zbiorów rozmytych, każda komórka opisana jest krotką: M[i,j] = {µ O (i,j),µ E (i,j),µ I (i,j),µ A (i,j),µ S (i,j)}. (3.91) Z komórką powiązanych jest n c punktów uzyskanych z pomiarów x pi = [x i y i ] T, i = 1...n c, które użyto do aktualizacji jej stanu. Przedstawiona na rys. 3.22 mapa komórek wspierających powstała na podstawie danych zebranych za pomocą skanera LMS 200 podczas eksperymentu opisanego dokładnie w podrozdziale 3.2.4 (por. rys. 3.10). Mapa ta odzwierciedla dość precyzyjnie kontury korytarza. Jednoznacznie są na niej zdefiniowane obszary, w których zgromadzono dane z sensorów wspierające hipotezę o istnieniu w tych obszarach przeszkód. Dane prowadzące do sprzecznych wartości O i E (komórka jednocześnie zajęta i wolna) zostały skutecznie usunięte. 136

3.4.3. Konwersja mapy rastrowej na reprezentację wektorowa Ponieważ robot działa w pomieszczeniach zamkniętych, jego otoczenie składa się w przeważającej mierze z elementów ograniczonych krawędziami prostoliniowymi. Do poszukiwania reprezentujących te krawędzie odcinków wykorzystano transformatę Hougha, działającą na mapie rastrowej traktowanej jak obraz w skali szarości, gdzie wartości określające stopień zajętości danej komórki odpowiadają wartościom funkcji obrazowej. Mapa rastrowa, w odróżnieniu od obrazu w skali szarości, charakteryzuje się tym, że komórki należące do poszukiwanych krawędzi mają określone wartości (wysoki stopień zajętości), w związku z czym nie ma potrzeby stosowania operatorów wyszukiwania krawędzi typowych dla przetwarzania obrazu, jednak w zależności od użytego sensora i jego modelu niepewności (por. p. 3.2) kontury obiektów mogą mieć znaczną szerokość, co utrudnia jednoznaczne wpasowanie cech wektorowych. W proponowanej jako reprezentacja pośrednia mapie rastrowej FSG kontury obiektów są grupami połączonych komórek o dużym stopniu przynależności do zbioru komórek wspierających S. Skali szarości odpowiadają wartości funkcji przynależności µ S. Proste obiekty geometryczne (np. linie, okręgi) można opisać za pomocą odpowiednich równań. Równania te charakteryzują się określoną liczbą parametrów. Każdy obiekt może być przedstawiony w przestrzeni wyznaczonej przez te parametry jako punkt. Linia prosta opisana jest równaniem normalnym icos(φ H ) + j sin(φ H ) = ρ H, (3.92) którego parametry φ H i ρ H tworzą tzw. przestrzeń Hougha. W wyniku dyskretyzacji tej przestrzeni otrzymuje się dwuwymiarową tablicę, której poszczególne komórki odpowiadają prostym (3.92) o różnych parametrach. Analizując obraz wejściowy (w tym przypadku mapę rastrową), znajduje się dla wszystkich komórek charakteryzujących się odpowiednio dużym poziomem zajętości (dla mapy FSG jest to wartość µ S większa od progu µ Smin ) parametry φ H i ρ H, dla których współrzędne komórki i,j spełniają równanie (3.92), a następnie inkrementuje się komórkę przestrzeni Hougha (zwanej też tablicą akumulatorów [298]) określoną przez te parametry. W tablicy akumulatorów znajduje się maksima będące odpowiednikami linii wspierających poszukiwanych cech geometrycznych. Końce odcinków znajduje się przez analizę sąsiedztwa tych komórek, które zostały zaliczone do danej prostej w przestrzeni Hougha. Problemem jest określenie niepewności znalezionego odcinka w formie zgodnej z obowiązującą dla mapy wektorowej. Podczas określania parametrów prostej w transformacie Hougha nie bierze się pod uwagę niepewności danych, a także nie wyznacza się bezpośrednio miary niepewności znalezionych linii. Samo zastosowanie transformaty wprowadza też dodatkową niepewność związaną z kwantyzacją przestrzeni parametrów. Konieczny jest dobór odpowiedniego stopnia dyskretyzacji przestrzeni Hougha. Podział tablicy akumulatorów na zbyt duże przedziały (zgrubna dyskretyzacja) powoduje zmniejszenie wierności odwzorowania otoczenia, gdyż niektóre cechy geometryczne znajdujące się na mapie rastrowej mogą nie być odnalezione. Natomiast zbyt gęsta kwantyzacja przestrzeni parametrów może dać liczne maksima lokalne, a co za tym idzie, wiele prostych o podobnych parametrach. Rezultatem działania transformaty Hougha są wówczas wielokrotne linie w miejscach, gdzie na mapie rastrowej znajduje się pojedyncza krawędź. Zjawisko to można do pewnego stopnia wyeliminować przez zastosowanie filtracji wartości w tablicy akumulatorów. Postępowanie takie, określane mianem Non-Maxima Suppression [137], polega na iteracyjnym zastosowaniu do wszystkich komórek akumulatora kwadratowej maski (o wymiarach np. 3 3 komórki), wewnątrz której poszukuje się lokalnego maksimum, a pozostałe komórki są zerowane. Należy 137

zwrócić uwagę, że użycie zbyt dużej maski spowoduje utratę szczegółowości odwzorowania otoczenia. Utrata precyzji jest też charakterystyczna dla samej reprezentacji rastrowej, ponieważ na podstawie poszczególnych odczytów odległości są aktualizowane określone komórki mapy, a informacja o dokładnej lokalizacji zaobserwowanego punktu zostaje utracona. Ze względu na te niedogodności i specyfikę gromadzonych danych, pochodzących ze skanera laserowego, zastosowano dwustopniowy schemat przetwarzania mapy rastrowej, odmienny od typowego użycia transformaty Hougha w przetwarzaniu obrazu. Pierwszy etap stanowi transformata Hougha pełniąca funkcję mechanizmu grupującego współliniowe komórki. Na etapie drugim parametry linii wspierającej są wyznaczane za pomocą metody najmniejszych kwadratów. Parametry wektora x l = [ρ φ] T opisującego tę linię oraz jego macierz kowariancji C l są obliczane ze wzorów (3.65 3.67) i (3.69) na podstawie współrzędnych punktów pomiarowych przechowywanych w komórkach, które zostały zaliczone na podstawie transformaty Hougha do danej linii oraz macierzy kowariancji tych punktów. Transformata Hougha zapewnia segmentację zbioru danych na grupy punktów w przybliżeniu współliniowych. Ponieważ wszystkie punkty przypisane są do komórek mapy rastrowej (są w nich zawarte), to w danej grupie punktów współiniowych nie mogą wystąpić punkty będące błędami nadmiernymi, których odległość od prostej znalezionej za pomocą transformaty Hougha jest większa niż długość boku komórki mapy. Nie ma więc potrzeby stosowania algorytmów estymacji parametrów prostej odpornych na błędy nadmierne. Po wyznaczeniu parametrów linii wspierających końce znalezionych odcinków są określane na podstawie projekcji na nieskończoną prostą skrajnych punktów wyznaczonych w pomiarach. Wylicza się środek p f = [x f y f ] T i długość l f każdego odcinka. Ogólny algorytm proponowanej metody wyodrębniania cech geoemtrycznych jest przedstawiony w postaci poniższego pseudokodu. Algorytm 3.5 procedure FSGHOUGH(M,n i,n j,µ Smin,H, φ H, ρ H,p min) 1 begin 2 ρ Hmax = n 2 i + n2 180 j ; kmax:= φ H ; l max:= ρ Hmax φ H ; empty:=false; {inicjacja} 3 k,l H[k, l]:=0; {zerowanie tablicy akumulatorów} 4 for i:=1 to n i do 5 for j:=1 to n j do 6 if µ S(M[i, j]) > µ Smin then {komórka wspierająca} 7 for k:=0 to φ max do 8 begin 9 φ:=k φ; ρ:=i cos φ + j sin φ; 10 l:= ρ ρ H ; H[k, l]:=h[k, l]+1; 11 end; {koniec aktualizacji tablicy akumulatorów} 12 NONMAXIMASUPPRESSION(H); {filtracja tablicy akumulatorów} 13 while not empty do 14 begin 15 (k,l):=houghpeak(h,k max,l max); {wyszukiwanie maksimum} 16 φ H:=k φ ; ρ H:=l ρ H; {parametry nowej prostej} 17 H[k, l] := 0; {usuwanie znalezionego maksimum z tablicy} 18 I H:=SEGMENTS(M,ρ H,φ H,g max,d min); {wyszukiwanie odcinków na mapie} 19 if I H > p min then [ρ φ] T :=LINEFIT(I H ) {estymacja parametrów prostej} 20 else empty:=true; {liczba punktów poniżej minimum} 21 end; 22 end. 138

W zapisie tym M oznacza mapę FSG, H tablicę akumulatorów, a n i i n j oraz k max i l max określają odpowiednio wymiary obu tablic. Komórki zawierające końce odcinków na mapie rastrowej wyszukuje się za pomocą analizy ośmiosąsiedztwa z użyciem funkcji SEGMENTS. Parametry funkcji: g max i d min oznaczają odpowiednio: maksymalną odległość między kolejnymi komórkami zaliczonymi do odcinka i minimalną długość odcinka. Efektem wyszukiwania jest zbiór I H komórek należących do odcinków leżących na danej prostej. Zbiory te stanowią parametr wejściowy procedury estymacji parametrów linii wspierającej LINEFIT (linia 19). Przeszukiwanie tablicy akumulatorów za pomocą funkcji HOUGHPEAK trwa dopóty, dopóki znajdowane w niej proste odpowiadają wystarczająco licznym zbiorom punktów minimalna liczba punktów dana jest parametrem p min. Mapa rastrowa może powstać na podstawie pomiarów różnymi rodzajami sensorów odległości, jak to pokazano na przykładach w podrozdziale 3.2.4, jednak pomiary dokonane sonarami mają zbyt małą rozdzielczość kątową, by mogły być uwzględnione podczas budowy mapy wektorowej przedstawiającej kontury obiektów z wystarczającą precyzją (porównywalną z uzyskiwaną ze skanera). Jeżeli jednak dane z sonarów są dostępne, umożliwiają aktualizację wartości komórek, a to się przyczynia do wzrostu wiarygodności tych pomiarów dokonanych skanerem laserowym, które pozostają w zgodzie z pomiarami ultradźwiękowymi. Rysunek 3.23. Kolejne etapy przetwarzania danych prowadzące od mapy rastrowej do wektorowej Opisana metoda konwersji rastrowej reprezentacji otoczenia na reprezentację wektorową może być zastosowana nie tylko do mapy rastrowej opartej na zbiorach rozmytych. Badano skuteczność tej metody także w odniesieniu do map tworzonych metodą probabilistyczną oraz z wykorzystaniem teorii Dempstera-Shafera [150, 151, 274]. Jeżeli zastosowany formalizm budowy mapy rastrowej nie zapewnia w wystarczającym stopniu eliminacji błędnych pomiarów lub użyte dane charakteryzują się znaczną niepewnością (duże odchylenie stan- 139

dardowe pomiaru odległości lub duża szerokość kątowa wiązki), to kontury obiektów na mapie są rozmyte i przypominają bardzo grube, nierówne linie. W związku z tym proponowana metoda wyodrębniania cech geometrycznych może być uzupełniona etapem wstępnego przetwarzania mapy rastrowej, polegającym na zastosowaniu algorytmu szkieletyzacji [298]. Transformatę Hougha wykonuje się wówczas na obrazie binarnym (szkielecie), w którym komórki jednoznacznie należą lub nie należą do konturów obiektów. Kolejne etapy przetwarzania pozostają bez zmian. Na rysunku 3.23 pokazano kolejne etapy przetwarzania danych ze skanera laserowego KARiI prowadzące do uzyskania mapy rastrowej, a następnie jej konwersji na postać wektorową. Na rysunku 3.23A przedstawiono dane ze skanera KARiI zebrane podczas eksperymentu, w czasie którego robot był pozycjonowany jedynie za pomocą odometrii. Poszczególne punkty zaznaczono okręgami o promieniu 2σ r, kolor szary oznacza punkty odrzucone podczas etapów wstępnego przetwarzania i grupowania. Na rysunku 3.23B zamieszczono mapę rastrową uzyskaną metodą probabilistyczną (tylko obszary zajęte), a na rys. 3.23C tę samą mapę po szkieletyzacji i uciągleniu. Na rysunku 3.23D pokazano proste znalezione za pomocą transformaty Hougha, a na kolejnych (3.23E i 3.23F) znalezione odcinki oraz te same odcinki po eliminacji cech niespełniających założonych kryteriów minimalnej długości i liczby wykorzystanych komórek mapy. Użycie mapy rastrowej utworzonej metodą zbiorów rozmytych oraz identyfikacja komórek wspierających cechy wektorowe (mapa FSG) pozwala uzyskać zadowalające wyniki ekstrakcji cech wektorowych nawet w sytuacji zakłóceń spowodowanych obiektami dynamicznymi. Ilustruje to rys. 3.24, na którym przedstawiono odcinki wyodrębnione proponowaną metodą z mapy rastrowej utworzonej metodą rozmytą w podstawowej wersji (wzory 3.46, 3.47) oraz z mapy FSG (wzór 3.90). W obu przypadkach identyfikacja cech odzwierciedlających ściany korytarza była prawidłowa, jednak wyniki ekstrakcji cech geometrycznych z rozmytej mapy obszarów zajętych (rys. 3.24A) charakteryzują się wieloma artefaktami spowodowanymi przez ruchomy obiekt (rys. 3.24B). Zastosowanie mapy FSG (rys. 3.24C) umożliwiło eliminację pomiarów pochodzących od obiektu ruchomego i w rezultacie wyniki użycia transformaty Hougha są znacznie lepsze (rys. 3.24D). Ponieważ dane wykorzystane w tym eksperymencie pochodzą ze skanera LMS 200, charakteryzującego się niewielką niepewnością pomiarów odległości, a robot dokonywał samolokalizacji podczas jazdy, kontury obiektów na mapie rastrowej są wyraźne i jednoznaczne (por. rys. 3.10). Z tego powodu nie stosowano szkieletyzacji. Użycie transformaty Hougha do wyodrębniania cech z mapy FSG pozwala odtworzyć podstawową geometrię otoczenia (ściany pomieszczenia) nawet z danych zawierających bardzo wiele zakłóceń. Dowodzą tego wyniki dwóch kolejnych eksperymentów, prowadzonych w pomieszczeniu laboratorium z użyciem robota wyposażonego w skaner LMS 200. Podczas pierwszego eksperymentu robot nie przemieszczał się, tylko obserwował wybrany fragment otoczenia. W polu widzenia skanera pojawiały się kolejno obiekty dynamiczne: przechodzące osoby i przesuwane z różną prędkością płyty styropianowe. Na mapie obszarów zajętych utworzonej z wykorzystaniem teorii zbiorów rozmytych ślady ruchomych obiektów są wyraźnie widoczne (rys. 3.25A). Obszary odpowiadające położeniu obiektów ruchomych zostały zidentyfikowane na mapie komórek niejednoznacznych (rys. 3.25B). Zastosowanie reguły łączenia informacji, wyrażonej wzorem (3.90), pozwoliło utworzyć mapę FSG, na której określono komórki wspierające stabilne cechy wektorowe (rys. 3.25C). Rozróżnienie między komórkami zawierającymi wiarygodne dane a komórkami, które nie należą do zbioru zdefiniowanego wzorem (3.90), jest wyraźnie widoczne na rys. 3.25D, na 140

A C B D Rysunek 3.24. Zastosowanie transformaty Hougha do ekstrakcji cech geometrycznych z mapy FSG (opis w tekście) A B C D E F Rysunek 3.25. Tworzenie i konwersja mapy rastrowej stacjonarny robot i dynamiczne środowisko którym mapę FSG przedstawiono w postaci trójwymiarowej. Użycie transformaty Hougha (rys. 3.25E) pozwala otrzymać mapę wektorową, która prawidłowo odzwierciedla geometrię otoczenia (rys. 3.25F). W eksperymencie drugim robot poruszał się po linii prostej w kierunku ściany. W polu widzenia skanera znajdowały się osoby przechodzące przez pomieszczenie oraz przemieszczane przez nie przedmioty. Na rysunku 3.26A widoczna jest mapa obszarów zajętych, a na rys. 3.26B mapa FSG, której trójwymiarową wizualizację przedstawiono na rys. 3.26C. Podobnie jak w poprzednim przypadku, zastosowanie transformaty Hougha do mapy FSG (rys. 3.26E) pozwala uzyskać cechy odzwierciedlające podstawową geometrię otoczenia (rys. 3.26F). Dla porównania na rys. 3.26D przedstawiono proste znalezione za pomocą transformaty Hougha na podstawowej mapie obszarów zajętych. Jak widać, użycie mapy FSG daje znacznie lepsze wyniki. 141

A B C D E F Rysunek 3.26. Tworzenie rozmytej mapy rastrowej w środowisku dynamicznym i jej konwersja na reprezentację wektorową (opis w tekście) Metoda wyodrębniania odcinków z mapy rastrowej FSG za pomocą transformaty Hougha pozwala odtworzyć kontury statycznych elementów otoczenia (w praktyce przede wszystkim ściany pomieszczeń) nawet z pomiarów odległości, których znaczna część ma charakter zakłóceń. Zgromadzenie na mapie FSG większej ilości informacji, zebranych w różnych pozycjach przez robota, umożliwia identyfikację obszarów (komórek) zawierających dane niejednoznaczne. Dwuetapowy proces wyodrębniania linii wspierających, w którym transformata Hougha uzupełniona jest etapem estymacji parametrów na podstawie rzeczywistych współrzędnych zebranych punktów, umożliwia natomiast dokładne określenie pozycji cech geometrycznych oraz wyznaczenie ich niepewności w postaci macierzy kowariancji. W tej metodzie niepewność pozycji cech geometrycznych nie zależy od rozmiaru komórki mapy rastrowej. Przedstawione podejście do wyodrębniania cech może być stosowane jako metoda konwersji off-line. Wówczas globalna mapa rastrowa tworzona jest sukcesywnie podczas wykonywania zadania przez robota, a mapa wektorowa jest odtwarzana z rastrowej jako reprezentacja dogodniejsza do celów nawigacji robota [151, 274]. Gdy przedstawiona metoda stosowana jest on-line, procedura konwersji wykonywana jest cyklicznie po utworzeniu lokalnej mapy rastrowej [282]. W proponowanej metodzie wyodrębniania cech nie udało się jednak wyeliminować typowego dla zastosowań transformaty Hougha problemu doboru optymalnej kwantyzacji przestrzeni parametrów. W eksperymentach, w których korzystano z danych ze skanera LMS 200, zastosowano kwantyzację φ H = 1 i ρ H = 4 cm, by uzyskać szczegółowość odwzorowania kształtów obiektów porównywalną ze szczegółowością uzyskiwaną metodą bezpośredniej analizy danych, opisaną w podrozdziale 3.3. Na skutek takiej kwantyzacji na wynikowych mapach pojawiają się jednak zwielokrotnione odcinki (por. rys. 3.24D i 3.26F), a poziom odwzorowania geometrii otoczenia jest nadal mniej szczegółowy niż uzyskany metodą segmentacji poszczególnych skanów. Istotną wadą transformaty Hougha jest długi czas wykonywania obliczeń. Zależy on od wielkości mapy rastrowej, wielkości tablicy akumulatorów oraz liczby komórek zakwalifikowanych do analizy (komórek o µ S większym od założonego progu). Złożoność obliczeniowa 142

algorytmu transformaty (algorytm 3.5), bez uwzględnienia procedur filtracji akumulatora i wyszukiwania odcinków oraz operacji wykonywanych na punktach pobranych z komórek, wynosi O(n M + n S n φ + n l n φ n ρ ), gdzie n M = n i n j i jest liczbą komórek mapy rastrowej, n S jest liczbą komórek wspierających wyodrębnianie cech, n l liczbą znalezionych linii, a n φ i n ρ określają wielkość tablicy akumulatorów. Podczas analizy mapy 100 100 komórek, uzyskanej na podstawie 10 skanów sensora LMS 200, z użyciem tablicy akumulatorów o wymiarach 180 142 uzyskiwano czasy od 0,8 do 2,5 s (AMD Athlon 1800MHz) w zależności od liczby znalezionych prostych i odcinków. Czasy te są akceptowalne do przetwarzania mapy off-line, w praktyce uniemożliwiają jednak wykorzystanie proponowanej procedury w systemie jednoczesnego tworzenia mapy i samolokalizacji robota. 3.4.4. Tworzenie map wektorowych ze wsparciem lokalnej reprezentacji rastrowej Alternatywą dla transformaty Hougha w zastosowaniach wymagających wyodrębnianie cech geometrycznych z danych napływających w czasie rzeczywistym 1 są metody bezpośredniego przetwarzania pomiarów odległości ze skanera laserowego, przedstawione w podrozdziale 3.3. W metodach segmentacji opartych na zasadzie split-and-merge wykorzystuje się informację o uporządkowaniu pomiarów odległości w skanie. Metody te w stosunku do transformaty Hougha są lepsze zarówno pod względem szybkości działania, jak i dokładności oraz poprawności uzyskiwanych cech geometrycznych [251]. Biorąc pod uwagę zalety przedstawionej w podrozdziale 3.3 metody opartej na algorytmie IEPF, tj. szybkość działania i dokładność odtworzenia konturów obiektów w otoczeniu robota, oraz wykazaną w podrozdziale 3.4.3 przydatność mapy rastrowej FSG jako narzędzia oceny wiarygodności pomiarów, zaproponowano metodę wyodrębniania cech geometrycznych, która łączy zalety obu tych podejść. W przeciwieństwie do pomiarów odległości w pojedynczym skanie punkty przechowywane w komórkach mapy rastrowej nie są uporządkowane, wobec czego do ich segmentacji nie można użyć algorytmu IEPF. W proponowanej metodzie ekstrakcji cech należy więc brać pod uwagę poszczególne skany i wykorzystywać budowaną na bieżąco mapę rastrową jako dodatkowe źródło informacji, pozwalające klasyfikować napływające dane jako wiarygodne lub błędne. Zarówno w procedurze aktualizacji mapy FSG, jak i w procedurze ekstrakcji cech bezpośrednio ze skanów wykorzystuje się to samo (fizyczne) źródło informacji skaner laserowy. Procedury te nie muszą jednak działać synchronicznie, co odpowiada wymaganiom czasowym narzucanym przez metodę jednoczesnej samolokalizacji i tworzenia mapy geometrycznej. Ze względu na dużą złożoność obliczeniową etapu aktualizacji algorytmu SLAM, opartego na rozszerzonym filtrze Kalmana (por. p. 4.2), nie jest możliwe przetwarzanie cech wyodrębnionych ze skanów z taką częstotliwością, z jaką skany te są wykonywane. Konieczna jest redukcja strumienia danych napływających ze skanera laserowego robota przez pominięcie części skanów (ang. subsampling). Ponieważ podczas jednoczesnej samolokalizacji i tworzenia mapy czas aktualizacji mapy jest proporcjonalny do kwadratu liczby cech znajdujących się na tej mapie, nowe cechy są wyodrębniane jedynie w wybranych punktach ścieżki lub w określonych odstępach czasu. Wykorzystywane są wybrane skany, natomiast informacja z pozostałych jest tracona. 1 Działanie w czasie rzeczywistym rozumiane jest jako zdolność do przetwarzania danych z częstotliwością nie mniejszą niż częstotliwość ich akwizycji. 143

Złożoność obliczeniowa procedury aktualizacji mapy rastrowej jest funkcją liczby komórek na mapie oraz liczby pomiarów w skanie. W przypadku tworzenia mapy lokalnej na podstawie danych ze skanera laserowego obie te wielkości są stałe, a więc czas aktualizacji mapy FSG jest stały podczas wykonywania zadania przez robota, niezależnie od wielkości globalnej mapy geometrycznej. Lokalna mapa FSG może być więc aktualizowana z częstotliwością równą częstotliwości akwizycji kolejnych skanów i z wykorzystaniem wszystkich skanów. Pomiary odległości uzyskane pomiędzy dwoma kolejnymi wywołaniami procedury ekstrakcji cech nie są przechowywane na mapie rastrowej, lecz służą do aktualizacji wartości funkcji przynależności do zbiorów rozmytych w jej komórkach. W ten sposób wszystkie pomiary przyczyniają się do identyfikacji komórek mapy w wystarczającym stopniu wspierających hipotezę o istnieniu cechy geometrycznej na danym obszarze. Punkty kolejnego skanu, użytego do wyodrębniania cech, uznawane są za wiarygodne, jeżeli leżą w komórkach o wystarczająco wysokim poziomie wsparcia wyznaczonego ze wzoru (3.90). Cechy w postaci odcinków są ekstrahowane z uporządkowanych zbiorów punktów tworzących skan. Jedynie punkty, które leżą w komórkach mapy FSG o stopniu przynależności do zbioru S większym od założonego progu, są uwzględniane na kolejnych etapach przetwarzania. W rezultacie powyższego postępowania za wiarygodne są uznawane te punkty, których położenie potwierdza wyniki poprzednich pomiarów. Pozwala to zidentyfikować obszary występowania statycznych struktur otoczenia (ściany, meble), których krawędzie stanowią cechy wykorzystywane do tworzenia mapy i samolokalizacji. Skaner LMS 200, w który jest wyposażony robot TRC Labmate, dokonuje pomiarów z rozdzielczością kątową 0,5, dostarcza 361 pomiarów odległości w pojedynczym skanie. Dane przesyłane są z prędkością 38 400 bps, co pozwala uzyskać maksymalną częstotliwość skanowania 4 Hz. Jeżeli robot porusza się ze średnią prędkością 0,2 m s, a procedura ekstrakcji cech geometrycznych wykonywana jest w punktach ścieżki odległych od siebie o 0,5 m, to opisany powyżej schemat akwizycji i przetwarzania danych pozwala uzyskiwać zestaw nowych cech geometrycznych co dziesięć cykli aktualizacji mapy FSG. Procedura bezpśredniego wyodrębniania odcinków, wspomagana mapą FSG, w ogólnych zarysach odpowiada metodzie bezpośredniej ekstrakcji cech geometrycznych opisanej szczegółowo w podrozdziale 3.3. Grupowanie pomiarów z zastosowaniem algorytmu 3.2 wykonywane jest dla każdego skanu, co pozwala usunąć niektóre błędne pomiary oraz poprawia jakość tworzonej mapy rastrowej. Danymi wejściowymi procedury segmentacji są grupy pomiarów reprezentujących odrębne obiekty. Pomiary te są przeliczane do współrzędnych kartezjańskich w układzie robota, a następnie do współrzędnych globalnych z uwzględnieniem aktualnej pozycji robota x R. Niepewność położenia punktu reprezentowana jest przez macierz kowariancji C p. Położenie punktów skanu porównywane jest z aktualną mapą FSG utworzoną przez robota na podstawie poprzednich skanów. Punkty znajdujące się w komórkach, dla których wartość funkcji przynależności µ S znajduje się poniżej zadanego progu, są odrzucane jako mało wiarygodne. Pozostałe grupy reprezentujące poszczególne obiekty w polu widzenia skanera są poddawane segmentacji na podgrupy punktów w przybliżeniu współliniowych za pomocą algorytmu IEPF. Parametry linii wspierających (3.63) poszczególnych cech geometrycznych wyznacza się za pomocą algorytmu 3.3, opartego na metodzie najmniejszych kwadratów (wzory 3.65 3.67). Macierze kowariancji (3.68) wylicza się ze wzoru (3.69). Początek i koniec odcinka ustala się przez rzutowanie odpowiednio pierwszego i ostatniego punktu należącego do danej cechy na jej linię wspierającą. Na podstawie tych danych wyznacza się środek odcinka p f i 144

jego długość l f. Podobnie jak w bezpośredniej analizie skanów, niepewność długości odcinka nie jest reprezentowana. Złożoność obliczeniowa proponowanej metody jest determinowana przez złożoność jej poszczególnych etapów: grupowania, segmentacji i estymacji parametrów linii wspierających. Złożoność algorytmu grupowania jest liniowa, a segmentacji metodą split-and-merge liniowo-logarytmiczna. Estymacja parametrów metodą najmniejszych kwadratów wyrażona jest w postaci zamkniętych formuł (3.65 3.67), lecz złożoność odpornego algorytmu estymacji (algorytm 3.3) jest wielomianowa. Jest to jednak pesymistyczne oszacowanie złożoności. Większość błędów nadmiernych w danych pochodzących ze skanera laserowego eliminowana jest za pomocą mapy rastrowej, na etapie estymacji pozostają głównie artefakty, których źródłem jest niedoskonałość algorytmu segmentacji. Ponieważ ich liczba jest mała, typowy czas wykonania procedury opisanej algorytmem 3.3 jest jedynie około dwukrotnie dłuższy niż podstawowej procedury LINEFIT. Rysunek 3.27. Ekstrakcja cech geometrycznych ze skanów wykonanych w środowisku dynamicznym (opis w tekście) Na rysunku 3.27 przedstawiono przykładowe rezultaty wyodręniania cech opisaną powyżej metodą i porównano je z wynikami uzyskanymi bez zastosowania pomocniczej reprezentacji rastrowej. W eksperymencie wykorzystano dane otrzymane w środowisku dynamicznym ze stacjonarnego (nieruchomego) robota (por. rys. 3.25). Górny rząd rysunku obrazuje cechy wyodrębnione metodą opisaną w podrozdziale 3.3, rząd środkowy punkty przetwarzanych skanów nałożone na mapę FSG (kolor biały oznacza komórki o wysokim stopniu przynależności do S), a rząd dolny cechy geometryczne uzyskane z uwzględnieniem mapy rastrowej. We wszystkich przypadkach proponowana metoda pozwoliła wyeliminować pomiary odnoszące się do obiektów dynamicznych, odpowiednio osoby przechodzącej w polu widzenia skanera (rys. 3.27A) oraz przemieszczanych płyt styropianowych różnej wielkości (rys. 3.27B, C, D). Ponieważ mapa FSG zapewnia filtrację nie tylko danych związanych ze środowiskiem dynamicznym, lecz także innych błędnych pomiarów (np. powstałych w wy- 145

niku efektu śladu optycznego), jej stosowanie pozwala na pewne rozluźnienie (relaksację) kryteriów odrzucania punktów podczas grupowania i segmentacji, co w rezultacie daje mapę wektorową zawierającą więcej szczegółów. Rysunek 3.28. Ekstrakcja cech geometrycznych ze skanów wykonanych przez poruszającego się robota (opis w tekście) Na rysunku 3.28 zaprezentowano wyniki wyodrębniania cech z danych uzyskanych w środowisku dynamicznym przez poruszającego się robota, który pozycjonowany był wyłącznie za pomocą odometrii. Także w tym przypadku zastosowanie pomocniczej reprezentacji rastrowej pozwoliło usunąć punkty będące zakłóceniami (rys. 3.28A). Jednak eksperyment z danymi uzyskanymi z robota będącego w ruchu wykazał dużą wrażliwość proponowanej metody na błędy pozycjonowania robota. Leżące u podstaw tej metody założenie o pokrywaniu się kolejnych punktów pochodzących z prawidłowych pomiarów odległości od obiektów statycznych jest spełnione jedynie wówczas, gdy pozycja robota określona jest z wystarczającą precyzją. Błędy pozycji robota powodują, że odrzucane są prawidłowe pomiary, gdyż nie znajdują się one w komórkach mapy wspierających hipotezę o istnieniu cechy geometrycznej (rys. 3.28B i C, obszary błędnego przyporządkowania pomiarów zaznaczono strzałkami). Zakres tolerancji błędów pozycjonowania zależy od przyjętej wielkości komórek mapy. Większe komórki pozwalają tolerować większe błędy pozycjonowania robota, natomiast mniejsze komórki umożliwiają precyzyjniejsze rozgraniczenie prawidłowych i błędnych pomiarów. Jednak dopiero dokładne informacje o pozycji robota zapewniają prawidłowe wykorzystanie pomocniczej mapy rastrowej. Propozycję rozwiązania tego problemu przedstawiono w podrozdziale 4.2. Przykładowe wyniki ekstrakcji cech geometrycznych z danych uzyskanych podczas eksperymentu, w trakcie którego robot dokonywał samolokalizacji, zaprezentowano na rys. 3.29. Wykorzystano dane, które posłużyły uprzednio do badania transformaty Hougha (por. rys. 3.24). Na rysunku 3.29A przedstawiono dane ze skanera LMS 200, a na rys. 3.29B wyniki wyodrębniania cech bezpośrednio z tych danych. Z kolei na rys. 3.29C widoczna jest lokalna mapa FSG, na którą nałożono przetwarzany skan, a na rys. 3.29D cechy geometryczne uzyskane po uwzględnieniu informacji z tej mapy. Przykład ekstrakcji cech geometrycznych z innego skanu uzyskanego w tym samym eksperymencie zaprezentowano na sekwencji rysunków 3.29E 3.29H. W porównaniu z metodą podstawową (bez mapy rastrowej) lokalne mapy geometryczne uzyskane dzięki zastosowaniu 146

Rysunek 3.29. Segmentacja przykładowych skanów podczas tworzenia mapy i samolokalizacji (opis w tekście) proponowanej hybrydowej reprezentacji informacji przestrzennej pozbawione są błędnych cech geometrycznych związanych z obiektami dynamicznymi. Ponadto, mapy te charakteryzują się większą precyzją umiejscowienia cech, chociaż nie pojawiają się na nich niektóre cechy odtworzone metodą podstawową. W proponowanej tu metodzie cechy są wyodrębniane, jeżeli na mapie FSG zgromadzona została wystarczająca ilość danych wspierających hipotezę o ich istnieniu. W związku z tym podczas sekwencyjnego przetwarzania skanów zbieranych przez robota niektóre nowe cechy pojawiają się z niewielkim opóźnieniem w porównaniu z metodą podstawową, w której zawsze przetwarzane są wszystkie dane ze skanu. Zjawisko to może mieć negatywny wpływ na wyniki samolokalizacji jedynie w otoczeniu charakteryzującym się niewielką liczbą naturalnych cech geometrycznych.

Rozdział 4 Autonomiczna nawigacja na podstawie danych uzyskanych z sensorów 4.1. Zagadnienie samolokalizacji robota mobilnego Określanie położenia i orientacji (pozycji) pojazdu w otaczającym go świecie jest klasycznym problemem robotyki mobilnej. Proces określania pozycji nosi miano pozycjonowania lub lokalizacji, a gdy robot określa swą pozycję na podstawie pomiarów własnymi sensorami, mówi się o samolokalizacji. Propozycję formalnego opisu zadania lokalizacji można znaleźć w rozprawie [236]. Dokładna znajomość własnej pozycji jest niezwykle istotna dla autonomicznego robota, gdyż umożliwia planowanie kolejnych działań i ruchów w znanym otoczeniu [186]; w otoczeniu nieznanym (całkowicie lub częściowo) jest warunkiem koniecznym utworzenia metrycznej mapy (rastrowej lub wektorowej) tego otoczenia (por. p. 3.3.6). Istnieje wiele metod korygowania pozycji robota na podstawie wyników pomiarów sensorami zewnętrznymi. Ich duża różnorodność wynika z konieczności dostosowania metody pozycjonowania do cech układu lokomocyjnego robota, rodzaju wykorzystywanych sensorów, właściwości otoczenia, w którym robot będzie działać, oraz typu wykonywanych zadań. Wiele komercyjnych robotów mobilnych, szczególnie przemysłowych AGV, wykorzystuje do pozycjonowania specjalne systemy sensoryczne, przeznaczone wyłącznie do tego zadaniu, a często także sztuczne znaczniki (landmarki) rozmieszczone w środowisku działania robota. Znacznikami sztucznymi nazywane są charakterystyczne elementy otoczenia umieszczone w nim celowo i służące wyłącznie do wspomagania pozycjonowania robotów. Stosowane są znaczniki aktywne (laserowe, radiowe) oraz pasywne, tj. odbłyśniki wykrywane przez aktywne sensory laserowe lub znaki graficzne obserwowane przez kamery wizyjne [26, 222]. Istnieje też możliwość obserwacji robota z zewnątrz, np. za pomocą kamery monitorującej otoczenie, i wyznaczenia w ten sposób jego pozycji [37, 39, 136, 171, 184]. Użycie sensorów związanych z otoczeniem lub znaczników nawigacyjnych pozwalających na szybkie i niezawodne wyznaczania pozycji pojazdu jest uzasadnione w wielu zastosowaniach robotów mobilnych (por. rozdz. 6), nie zawsze jednak instalacja tego rodzaju pomocy 148

nawigacyjnych jest możliwa. Mając to na uwadze, podczas projektowania systemu nawigacji autonomicznego robota mobilnego dąży się do ograniczenia, a docelowo wyeliminowania konieczności modyfikowania jego otoczenia. Zadanie pozycjonowania robota autonomicznego staje się wówczas zadaniem samolokalizacji i jest realizowane w oparciu o dane z sensorów zewnętrznych (dalmierzy, skanerów, kamer), wykrywających naturalne cechy otoczenia. Do samolokalizacji można wykorzystać metody globalne, umożliwiające robotowi jednoznaczne określenie swojej pozycji względem globalnego układu odniesienia [236], albo metody lokalne, pozwalające jedynie na korekcję istniejącego oszacowania pozycji w układzie globalnym. Dokładny przegląd aktualnego stanu wiedzy w dziedzinie systemów pozycjonowania robotów mobilnych wykracza poza zakres niniejszej rozprawy. Wyczerpujące, choć nie najbardziej aktualne, omówienie sensorów stosowanych do pozycjonowania robotów oraz wykorzystywanych technik i metod zainteresowany czytelnik znajdzie w opracowaniu [106]. Znane z literatury metody pozycjonowania można usystematyzować, biorąc pod uwagę różne kryteria. Próbę takiej klasyfikacji przedstawiono w opracowaniu [106], a w odniesieniu do wybranych grup algorytmów samolokalizacji także w pracach [123, 141, 236]. Podobnie jak inne funkcje systemu nawigacji, samolokalizacja wymaga odwoływania się do informacji przestrzennej opisującej otoczenie. Informacja ta, w postaci mapy, pozwala umiejscowić wykryte cechy otoczenia w określonym układzie odniesienia, a więc także ustalić pozycję robota w tym układzie. Jeżeli mapa dana jest a priori (predefiniowana), robot może dokonać samolokalizacji przez skojarzenie pewnych cech środowiska obserwowanych w danej chwili (mapy lokalnej) z cechami tego samego rodzaju zapisanymi na posiadanej mapie otoczenia. W literaturze opisano wiele metod opartych na tej zasadzie, istotnie różniących się postacią predefiniowanej mapy, rodzajem cech wyodrębnianych z danych sensorycznych oraz sposobem ich dopasowywania [77, 236, 253, 254, 273]. Wykorzystując dodatkowo samolokalizację zliczeniową (odometrię), można ograniczyć obszar mapy globalnej, w którym poszukiwane jest dopasowanie. Wówczas są porównywane cechy wyodrębnione z aktualnych odczytów sensorów z cechami, których wykrycie jest oczekiwane w danym momencie na podstawie znanego modelu otoczenia i dostępnej estymaty pozycji robota uzyskanej metodą samolokalizacji zliczeniowej. Taki schemat rozwiązania zadania samolokalizacji robota autonomicznego jest obecnie dominujący, a aparatem matematycznym najczęściej wykorzystywanym do realizacji tego zadania jest rozszerzony filtr Kalmana (EKF) [22, 23, 141, 167, 180, 181, 220]. Problemem pozostaje uzyskanie kompletnego i dokładnego modelu otoczenia, wystarczającego do precyzyjnej samolokalizacji. Jest to zadanie pracochłonne, a utrzymanie tego modelu w stanie odzwierciedlającym aktualny stan środowiska może się okazać niemożliwe. Ponadto, mapa utworzona na podstawie innych danych niż pomiary dokonywane przez sensory robota (zazwyczaj są to ręczne pomiary odległości lub dostępne plany architektoniczne) nie odzwierciedla wszystkich cech wykrywanych przez dany rodzaj sensora. Rozwiązaniem tego problemu jest jednoczesna samolokalizacja i tworzenie mapy otoczenia (SLAM). O ile tworzenie mapy przy znanej pozycji robota oraz samolokalizacja (lokalna) w znanym otoczeniu są zadaniami stosunkowo łatwymi z algorytmicznego punktu widzenia, wymagającymi jedynie zastosowania skutecznych metod analizy danych napływających z sensorów, o tyle zadanie jednoczesnej samolokalizacji robota mobilnego i tworzenia mapy jest trudne, charakteryzuje się dużą złożonością obliczeniową i pamięciową [90, 307] oraz wymaga szczególnej uwagi w zakresie modelowania procesów percepcji (modele sensorów) i akcji (model robota). 149

W kolejnych podrozdziałach poddano analizie wybrane aspekty realizacji systemu SLAM, skupiając uwagę na wykorzystaniu modeli sensorów wprowadzonych w rozdziale 2 oraz metod przetwarzania danych zaproponowanych w rozdziale 3, co pozwala rozpatrywać SLAM jako rozwinięcie opisanego w podrozdziale 3.3 systemu tworzenia mapy wektorowej przy znanej pozycji robota. W przedstawionych eksperymentach jest wykorzystywany skaner LMS 200. Skaner laserowy jest obecnie najczęściej stosowanym sensorem w zadaniu samolokalizacji robota w oparciu o naturalne cechy otoczenia [77, 130, 273] oraz w zadaniu SLAM [23, 72, 90, 141, 209, 219]. 4.2. Jednoczesna samolokalizacja i tworzenie mapy otoczenia 4.2.1. Zagadnienie SLAM Mianem jednoczesnej lokalizacji i tworzenia mapy (SLAM) określane są dwa ściśle związane ze sobą problemy: budowa modelu otoczenia i samolokalizacja robota mobilnego. Mapa tworzona jest na podstawie odczytów sensorów określających położenie obiektów w otoczeniu względem układu współrzędnych robota. By utworzyć spójną w sensie topologicznym mapę otoczenia, wyniki wszystkich pomiarów muszą być umieszczone we wspólnym, zewnętrznym względem robota, układzie odniesienia, co implikuje konieczność dysponowania dokładną informacją o pozycji robota w tym układzie. W procesie samolokalizacji robot wyznacza swoją pozycję na podstawie pomiarów określających jego położenie i orientację względem pewnych obiektów w otoczeniu. Znajomość położenia tych obiektów w układzie współrzędnych zewnętrznym względem robota pozwala określić jego pozycję w tym układzie. Do samolokalizacji niezbędna jest więc robotowi mapa otoczenia zawierająca informacje o położeniu obserwowanych obiektów w zewnętrznym układzie odniesienia. W wielu metodach samolokalizacji opisanych w literaturze zakłada się istnienie znanej a priori mapy otoczenia [22, 77, 180, 236]. Także metody tworzenia mapy rozwijane są często z założeniem znajomości położenia i orientacji robota [78, 213, 228, 235, 243, 270], przy czym w praktyce korzysta się z odometrii [271] lub zewnętrznych względem robota systemów pozycjonowania [274, 282]. Jednak aby robot mobilny osiągnął rzeczywistą autonomię, musi mieć możliwość budowy modelu nieznanego otoczenia [181], co wymaga jednoczesnego rozwiązania problemu tworzenia mapy i samolokalizacji. Oba problemy są ściśle ze sobą splecione 1. W szczególności do realizacji obu zadań są wykorzystywane te same odczyty sensorów, określające relacje przestrzenne między robotem a otaczającymi go obiektami. Ponieważ dane z sensorów zawsze są niepewne, także położenie wykrytych obiektów, pozycja robota i relacje przestrzenne na mapie otoczenia zawsze będą obarczone niepewnością. Z tego powodu algorytm rozwiązujący problem SLAM powinien być oparty na formalizmie matematycznym pozwalającym tę niepewność informacji uwzględnić oraz odpowiednio reprezentować i propagować. Najwygodniejsze wydaje się zastosowanie metod i narzędzi probabilistyki. Takie podejście nie tylko ma ugruntowane podstawy teoretyczne, lecz także jest źródłem wielu metod pozwalających na analizę systemów dynamicznych. Do tego rodzaju metod, wykorzystywanych do rozwiązywania zagadnień samolokalizacji i tworzenia mapy, należą między innymi: filtr Kalmana, dynamiczne 1 Sytuacja określana w literaturze anglojęzycznej mianem chicken-and-egg problem. 150

sieci bayesowskie, filtr Bayesa, filtracja cząsteczkowa, częściowo obserwowalne procesy decyzyjne Markowa (POMDP). Sformułowane w ogólny sposób zadanie SLAM polega na znalezieniu takiej pozycji robota x R(k) i takiej mapy otoczenia m (k) w chwili k, które są najbardziej prawdopodobne w świetle danych sensorycznych zgromadzonych do chwili k i wykonanych do tej chwili poleceń sterujących robotem. Podstawowe założenia odnośnie do stanu robota (pozycji), obserwacji i sterowania są następujące: pozycja robota x R jest łańcuchem Markowa procesem stochastycznym z czasem dyskretnym, który ma własność Markowa, polegającą na tym, że warunkowy rozkład prawdopodobieństwa P(x R(k+1) ) jest wyłącznie funkcją x R(k) : P(x R(k+1) x R(1),x R(2),...x R(k) ) = P(x R(k+1) x R(k) ); (4.1) obserwacje z w kolejnych chwilach są statystycznie niezależne względem innych obserwacji: i=1...k, j=1...k P(z i,z j ) = P(z i )P(z j ); (4.2) polecenia sterujące u w kolejnych chwilach są statystycznie niezależne względem innych poleceń sterujących: i=1...k, j=1...k P(u i,u j ) = P(u i )P(u j ); (4.3) obserwacje z w kolejnych chwilach są statystycznie niezależne względem sterowania u w tych samych chwilach: i=1...k P(z i,u i ) = P(z i )P(u i ). (4.4) W probabilistycznym sformułowaniu zadania SLAM zakłada się, że stan systemu obejmujący zarówno pozycję robota x R, jak i mapę m w każdej chwili reprezentowany jest przez określoną funkcję gęstości prawdopodobieństwa [209]. By rozwiązać problem SLAM, należy znaleźć funkcję gęstości prawdopodobieństwa a posteriori w chwili k przy założeniu, że znane są wszystkie obserwacje do tej chwili z k = {z (1),z (2),...z (k) } i polecenia sterujące u k = {u (1),u (2),...u (k) }. Poszukiwany rozkład zdefiniowany jest w następujący sposób: p(x R(k),m (k) z k,u k ). (4.5) Jednym z podstawowych narzędzi probabilistyki wykorzystywanych do przetwarzania informacji niepewnej jest reguła Bayesa (3.3), która pozwala wyznaczyć nową unkcję gęstości prawdopodobieństwa danej zmiennej losowej, gdy dostępna jest nowa informacja dotycząca tej zmiennej. Zastosowanie reguły Bayesa w odniesieniu do prawdopodobieństw zdarzeń losowych będących wartościami określonego markowowskiego procesu stochastycznego w każdym kroku tego procesu nazywane jest rekursywną filtracją bayesowską [258]. Filtr Bayesa pozwala wyznaczyć funkcję gęstości prawdopodobieństwa a posteriori zmiennej losowej reprezentującej stan procesu stochastycznego na podstawie dostępnych informacji wpływających na ten stan, np. odczytów sensorów lub znanego sterowania. Filtr Bayesa 151

można wykorzystać do sformułowania ogólnego, probabilistycznego rozwiązania problemu SLAM [209, 307]. Stosując regułę Bayesa, funkcję gęstości prawdopodobieństwa (4.5) można przedstawić w następującej postaci: p(x R(k),m (k) z k,u k ) = ηp(z (k) x R(k),m (k),z k 1,u k )p(x R(k),m (k) z k 1,u k ), (4.6) gdzie η jest stałym czynnikiem normalizującym, który zapewnia, że zależność (4.6) jest gęstością prawdopodobieństwa. Ponieważ zakłada się, że każda obserwacja z i,i = 1...k, jest statystycznie niezależna od innych obserwacji (4.2) oraz że obserwacja w chwili k jest niezależna od sterowania w tej samej chwili (4.4), to wzór (4.6) można uprościć do postaci: p(x R(k),m (k) z k,u k ) = ηp(z (k) x R(k),m (k) )p(x R(k),m (k) z k 1,u k ). (4.7) Korzystając ze wzoru na prawdopodobieństwo całkowite oraz własności Markowa (4.1), funkcję gęstości prawdopodobieństwa (4.7) można zapisać jako zależną od poprzedniego stanu robota i otoczenia w chwili k 1: p(x R(k),m (k) z k,u k ) = = ηp(z (k) x R(k),m (k) ) p(x R(k),m (k) x R(k 1),m (k 1),z k 1,u k ) p(x R(k 1),m (k 1) z k 1,u k )dx R(k 1) dm (k 1). (4.8) Dotychczas rozpatrywany był najbardziej ogólny przypadek jednoczesnej samolokalizacji i tworzenia mapy, w którym dopuszczono, że mapa ewoluuje w czasie (podobnie jak pozycja robota jest ona procesem losowym). Przy takim założeniu stan mapy jest różny w każdym kroku estymacji prawdopodobieństwa (4.5), co prowadzi do równania (4.8). Wyznaczenie z tego równania funkcji gęstości prawdopodobieństwa a posteriori jest bardzo kosztowne obliczeniowo (sumowanie względem wszystkich map) oraz problematyczne w implementacji, gdyż w przeciwieństwie do modelu ruchu robota model opisujący ewolucję mapy z reguły nie istnieje (np. osoby w polu widzenia robota mogą poruszać się w sposób przypadkowy i przemieszczać dowolne przedmioty). Z powyższych powodów powszechnie przyjmuje się założenie o statycznym otoczeniu [307]. Takie też założenie przyjęto w dalszych rozważaniach dotyczących problemu SLAM. Na jego mocy mapa m nie zmienia się w czasie, wobec czego poszukiwana funkcja gęstości prawdopodobieństwa przyjmuje postać: a wzór (4.8) można istotnie uprościć: p(x R(k),m z k,u k ), (4.9) p(x R(k),m z k,u k ) = = ηp(z k x R(k),m) p(x R(k),m x R(k 1),z k 1,u k )p(x R(k 1) z k 1,u k )dx R(k 1). (4.10) Korzystając z definicji prawdopodobieństwa warunkowego, wzór (4.10) można przekształcić do postaci: p(x R(k),m z k,u k ) = = ηp(z (k) x R(k),m) p(x R(k) x R(k 1),m,z k 1,u k ) p(m x R(k 1),z k 1,u k )p(x R(k 1) z k 1,u k )dx R(k 1). (4.11) 152

Ponieważ pozycja robota w chwili k jest zależna wyłącznie od poprzedniej pozycji x R(k 1) (własność Markowa) oraz sterowania w chwili k, to funkcję gęstości prawdopodobieństwa można zapisać jako: p(x R(k),m z k,u k ) = = ηp(z (k) x R(k),m) p(x R(k) x R(k 1),u (k) ) p(m x R(k 1),z k 1,u k )p(x R(k 1) z k 1,u k )dx R(k 1). (4.12) Ponownie korzystając z definicji prawdopodobieństwa warunkowego, równanie (4.12) można przekształcić do postaci: p(x R(k),m z k,u k ) = = ηp(z (k) x R(k),m) p(x R(k) x R(k 1),u (k) )p(x R(k 1),m z k 1,u k )dx R(k 1). (4.13) Ponieważ sterowanie w chwili k nie ma wpływu na poprzednią pozycję robota x R(k 1), równanie (4.13) można ostatecznie przekształcić do postaci: p(x R(k),m z k,u k ) = model obserwacji model systemu poprzedni stan {}}{ = η p(z (k) x R(k),m) {}}{{}}{ p(x R(k) x R(k 1),u (k) ) p(x R(k 1),m z k 1,u k 1 ) dx R(k 1), (4.14) będącej rekursywną formą równania filtru Bayesa dla problemu SLAM. Równanie to pozwala wyznaczyć rozkład prawdopodobieństwa a posteriori stanu systemu (pozycji robota i mapy) w chwili k, jeżeli prawdopodobieństwo to znane było w chwili k 1 oraz dostępne są probabilistyczne modele ruchu robota i obserwacji. Równanie filtru Bayesa dla problemu SLAM w ogólnej postaci (4.14) nie może być użyte w praktyce, gdyż występująca w nim całka nie ma w ogólnym przypadku analitycznego rozwiązania [209]. Rozwiązanie jest możliwe po przyjęciu dodatkowych założeń co do postaci rozkładu prawdopodobieństwa stanu systemu, a także co do postaci modeli obserwacji i ruchu robota. Zasada filtracji Bayesa z odpowiednimi założeniami jest podstawą wszystkich znanych z literatury probabilistycznych algorytmów SLAM [307]. Równanie (4.14) obrazuje strukturę rozwiązania problemu SLAM i sposób wykorzystania poszczególnych źródeł informacji (pomiary, sterowanie). Porównanie tego ogólnego rozwiązania, wyprowadzonego przy minimalnej liczbie założeń, z rzeczywistym algorytmem rozwiązującym problem SLAM pozwala lepiej zrozumieć, jakie są konsekwencje przyjęcia konkretnych założeń i modeli matematycznych oraz jakie wynikają z tego ograniczenia. 4.2.2. Jednoczesna samolokalizacja i tworzenie mapy z użyciem EKF Przystępując do budowy systemu realizującego zadanie jednoczesnej samolokalizacji i tworzenia mapy, należy dokonać wyboru reprezentacji otoczenia. Wybór rodzaju mapy wpływa na postać stanu systemu, a także postać modelu obserwacji. W literaturze opisywane są probabilistyczne podejścia do rozwiązania problemu SLAM oparte na surowych danych z sensorów odległości [84, 267], mapach rastrowych [304] i mapach topologicznych [266]. Jednak efektywnym i uniwersalnym sposobem reprezentacji informacji przestrzennej dla robota mobilnego działającego w środowisku stworzonym przez człowieka jest mapa wektorowa, przedstawiona w podrozdziale 3.3. Tworzenie wektorowej mapy otoczenia jest względnie łatwe, jeżeli pozycja robota jest dokładnie znana. Pozycje poszczególnych obiektów (cech) można estymować niezależnie 153

z użyciem statycznego filtru Kalmana (por. p. 3.3). Jeżeli jednak pozycja robota nie jest znana, a tworzona mapa stanowi podstawę samolokalizacji, to błąd pozycji robota będzie w coraz większym stopniu skorelowany z błędami mapy otoczenia. Gdy w kolejnych krokach (kolejnych punktach ścieżki) robot estymuje położenie obiektów na mapie, biorąc za podstawę swoją aktualną pozycję, to błędy położenia obiektów na mapie będą skorelowane z błędem pozycji robota, a poprzez tę pozycję z błędami położenia innych, wcześniej obserwowanych obiektów. W rezultacie stan robota i stan wszystkich cech na mapie wektorowej muszą być estymowane jednocześnie. Mapa wektorowa może być użyta wraz z różnymi metodami rozwiązania problemu SLAM, np. opartymi na filtracji cząsteczkowej [140, 209], jednak najpopularniejsze obecnie podejście do rozwiązania zadania SLAM, w którym wykorzystuje się wektorową reprezentację otoczenia, zostało zapoczątkowane przez Smitha i współpracowników [290]. Zaproponowali oni probabilistyczną metodę modelowania relacji geometrycznych pomiędzy robotem a obiektami w jego otoczeniu wraz z niepewnością tych relacji. W metodzie tej: mapa otoczenia m, nazywana mapą stochastyczną, jest zbiorem cech geometrycznych opisanych wektorami losowymi; estymowany wektor stanu systemu x obejmuje pozycję robota i wszystkie cechy składające się na mapę; niepewność modelowana jest za pomocą macierzy kowariancji C x wektora stanu systemu; do aktualizacji wektora stanu i macierzy kowariancji wykorzystywany jest rozszerzony filtr Kalmana (stąd metoda ta określana jest mianem EKF-SLAM). Poniżej podano matematyczny opis metody EKF-SLAM, stanowiącej jedną z możliwych realizacji ogólnego, probabilistycznego rozwiązania problemu SLAM danego równaniem (4.14). Wskazano też na związek EKF-SLAM z opartymi na filtracji Kalmana rozwiązaniami prostszych problemów: samolokalizacji na podstawie znanej mapy i tworzenia mapy przy znanej pozycji robota. W algorytmie filtru Kalmana wykorzystuje się zasadę filtracji bayesowskiej jest on rekursywnym filtrem dla systemów liniowych, estymatorem wartości średniej minimalizującym wariancję błędu estymacji. Jest też zarazem filtrem optymalnym w klasie filtrów liniowych [12, 256]. Filtr Kalmana w standardowej postaci nie może być jednak użyty w metodzie samolokalizacji robota, ponieważ zarówno procesy pomiaru (model obserwacji), jak i ruchu robota (model systemu) opisane są równaniami nieliniowymi. W przypadku robota mobilnego poruszającego się po płaskiej powierzchni nieliniowość ujawnia się w składowych orientacji, występujących w równaniach stanu, tj. w równaniach (2.3) dla robota o różnicowym układzie jezdnym. Z tego powodu stosuje się rozszerzony filtr Kalmana (EKF) [30], w którym wykorzystuje się modele ruchu robota i obserwacji cech zlinearyzowane w otoczeniu bieżącej estymaty stanu robota. By posłużyć się algorytmem EKF do estymacji funkcji rozkładu gęstości prawdopodobieństwa (4.9), należy z góry założyć, że funkcja ta definiuje rozkład normalny. Założenie to można zapisać w następujący sposób: p(x R(k),m z k,u k ) N(ˆx (k),c x ), (4.15) 154

gdzie wektor x (k) reprezentuje bieżący stan systemu, złożony z wektora pozycji robota oraz n wektorów cech znajdujących się na mapie: x R(k) x F1(k) x(k) = x F2(k), (4.16). x Fn(k) a C x jest macierzą kowariancji wektora stanu: C R C RF1... C RFn C F1R C F1... C F1F n C x (k) =......... (4.17) C FnR C FnF 1... Należy zdefiniować także rozkłady prawdopodobieństwa reprezentujące model dynamiki systemu i model obserwacji. W większości zastosowań EKF model systemu opisuje sposób, w jaki dany układ zmienia stan w odpowiedzi na znane sterowanie [12]. Podejście takie w przypadku modelowania robota mobilnego odpowiadałoby wykorzystaniu równań kinematyki i nominalnych wartości zmiennych sterujących (zadanych prędkości kół) w chwili k 1. Robot mobilny dysponuje jednak układem odometrii, który umożliwia zazwyczaj lepsze oszacowanie przemieszczenia niż matematyczny model pojazdu. Z tego powodu wektor u nie jest sterowaniem znanym w chwili k 1, lecz pomiarem dokonywanym w chwili k (po zakończeniu ruchu) i jest wektorową zmienną losową o rozkładzie normalnym: C Fn u N(ū,C u ). (4.18) Dla robota o różnicowym układzie jezdnym macierz C u jest określona wzorem (2.25). Model obserwacji zdeterminowany jest przez sposób prognozowania pomiaru na podstawie stanu robota oraz informacji zawartych na mapie. Model ten opisany jest nieliniową funkcją zależną od estymaty stanu w chwili k oraz mapy m: ẑ (k) = h(ˆx R(k),m). (4.19) Funkcja h opisuje sposób wyznaczania obserwacji na podstawie mapy, umożliwia więc przekształcenie cech opisanych w układzie globalnym (mapy) w cechy opisane w układzie robota. Postać funkcji h zależy od rodzaju wykorzystywanego sensora oraz sposobu opisu cech na mapie otoczenia. Rzeczywista obserwacja (pomiar) opisana jest wektorową zmienną losową o rozkładzie normalnym: z N( z,c r ). (4.20) Procedura rozwiązania zadania SLAM przy opisanych wyżej założeniach o rozkładach prawdopodobieństw i postaci mapy estymuje wektor wartości średnich stanu i macierz kowariancji, które to parametry jednoznacznie określają rozkład (4.15). W procedurze aktualizacji funkcji gęstości prawdopodobieństwa danej równaniem (4.14) można wyróżnić dwa etapy: etap predykcji, na którym modyfikuje się rozkład prawdopodobieństwa z poprzedniego kroku na podstawie modelu dynamiki systemu, oraz etap korekcji, 155

na którym integruje się informacje pochodzące z obserwacji. Strukturę tę obrazuje poniższy zapis równania (4.14): p(x R(k),m z k,u k ) = etap predykcji {( }} ){ etap korekcji {}}{. = η p(x R(k) x R(k 1),u k )p(x R(k 1),m z (k 1),u (k 1) )dx R(k 1) p(z k x R(k),m) Operacja prognozowania parametrów nowego rozkładu prawdopodobieństwa na podstawie znajomości rozkładu poprzedniego i sterowania wykonywana jest za pomocą predykcyjnej postaci EKF: ˆx R(k k 1) = f(ˆx R(k 1 k 1),u (k) ), (4.21) ˆx Fi(k+1) = ˆx Fi(k), (4.22) C R(k k 1) = J fr C R(k 1 k 1) J T f R + J fu C u(k) J T f U, (4.23) C RFi(k k 1) = J fr C RFi(k 1 k 1), (4.24) gdzie funkcja f opisuje sposób, w jaki stan zmienia się pod wpływem sterowania u, J fr = f x i jest jakobianem funkcji f względem stanu robota, a J fu = ˆxR(k 1 k 1) f u u(k) i jest jakobianem tej funkcji względem sterowania. Znane sterowanie zostało zastąpione pomiarem przybliżonej pozycji robota dokonanym w chwili k. Ponieważ obowiązuje założenie o statycznym środowisku, predykcja dotyczy tylko tej części wektora stanu, która opisuje pozycję robota (4.21), natomiast parametry cech nie ulegają zmianie (4.22). Etap predykcji powoduje wzrost niepewności pozycji robota (4.23), gdyż zmianie stanu nie towarzyszy integracja nowych informacji z niezależnego (zewnętrznego) źródła. Na skutek przemieszczenia robota zmienia się też korelacja między jego pozycją a parametrami cech na mapie (4.23). Złożoność obliczeniowa etapu predykcji metody EKF-SLAM jest liniowa w funkcji liczby cech na mapie. Implementację etapu korekcji filtru Bayesa (4.14) stanowią równania estymatora stanu systemu. Na podstawie prognozowanego na poprzednim etapie stanu dokonywana jest predykcja obserwacji cech oraz ich macierzy kowariancji: ẑ 1(k) h(ˆx R,x F1 ) ẑ 2(k) ẑ (k) = h(ˆx (k k 1) ) =. = h(ˆx R,x F2 )., (4.25) ẑ n(k) h(ˆx R,x Fn ) C z(k) = J hx C x(k k 1) J T h X, (4.26) gdzie h jest funkcją dokonującą transformacji cech z układu globalnego (mapy) w cechy opisane w układzie robota, a J hx = h x i jest jakobianem funkcji h względem stanu. ˆx(k k 1) W układzie robota prowadzone są rzeczywiste obserwacje, polegające na ekstrakcji cech z danych sensorycznych. Cechy obserwowane w chwili k opisane są wektorami parametrów z i(k) i macierzami kowariancji C ri(k), gdzie i = 1...n f i jest indeksem cechy. W ogólnym rozwiązaniu zadania SLAM, przedstawionym w poprzednim podrozdziale, milcząco zakłada się, że jest rozwiązany problem kojarzenia danych pomiarowych z mapą (ang. data association problem). W rzeczywistej implementacji systemu jednoczesnej samolokalizacji robota i tworzenia mapy problem ten musi być rozwiązywany na bieżąco 156

dla każdej obserwacji z i(k). Skuteczne kojarzenie obserwacji i predykcji jest warunkiem koniecznym poprawnego działania systemu SLAM, gdyż niepewność pozycji robota może być zredukowana jedynie dzięki ponownej obserwacji cech już znajdujących się na mapie. Jeżeli identyfikacja tych cech nie będzie skuteczna, nie nastąpi także korekta prognozowanej estymaty pozycji robota. Ze względu na swą istotność problem kojarzenia danych w metodzie EKF-SLAM jest przedmiotem wielu prac badawczych [28, 199, 221]. Podstawową techniką kojarzenia predykcji i obserwacji cech jest jednak test, w którym wykorzystuje się kwadrat odległości Mahalanobisa (por. wzór 3.85). Warunek dopasowania (ang. validation gate) obserwacji do predykcji i-tej cechy ma następującą postać: (z (k) ẑ i(k) )(C zi(k) + C r(k) ) 1 (z (k) ẑ i(k) ) T χ 2 r,α, (4.27) gdzie próg χ 2 r,α jest określany na podstawie tablic rozkładu χ 2 dla założonego prawdopodobieństwa prawidłowego dopasowania α i liczby stopni swobody r odpowiadającej liczbie parametrów cechy, np. dla cechy punktowej o współrzędnych [x y] T lub dla prostej postaci (3.63) liczba stopni swobody r = 2. Podczas działania rzeczywistego systemu SLAM mogą zaistnieć sytuacje, w których wynik testu (4.27) nie jest jednoznaczny, czyli do danej obserwacji pasuje w sensie statystycznym więcej niż jedna cecha. Z tego względu test oparty na odległości Mahalanobisa uzupełniany jest innymi kryteriami dopasowania, zależnymi od postaci cech i właściwości systemu percepcji robota, np. dla cech w postaci odcinków są to testy geometryczne opisane w podrozdziale 3.3.6 (por. rys. 3.21). Konieczne jest także ustalenie zasady, na mocy której akceptowane są dopasowania niejednoznaczne. Najczęściej spotykanym rozwiązaniem jest zastosowanie zasady najbliższego sąsiada (ang. nearest neighbour), czyli wybór tej spośród prognozowanych cech, która spełnia warunek (4.27) i dodatkowe warunki geometryczne, a której odległość Mahalanobisa względem rozpatrywanej obserwacji jest najmniejsza [141, 180, 220]. Porównanie par dopasowanych obserwacji: prognozowanych i rzeczywistych pozwala skorygować wstępne oszacowanie stanu w chwili k, dokonane na etapie predykcji, przez integrację nowych danych pomiarowych dostępnych w chwili k i estymację stanu systemu: K (k) = C x(k k 1) J T h X (J hx C x(k k 1) J T h X + C r(k) ) 1, (4.28) ˆx (k k) = ˆx (k k 1) + K (k) (z (k) ẑ (k) ), (4.29) C x(k k) = (I K (k) J hx )C x(k k 1). (4.30) Etap ten charakteryzuje się wielomianową złożonością obliczeniową O(n 2 m), gdzie n jest liczbą cech na mapie, a m liczbą obserwacji dopasowanych do prognozowanych cech w danym kroku. Integracja posiadanych informacji z nowymi, pochodzącymi z zewnętrznego źródła, pozwala zredukować niepewność zarówno pozycji robota, jak i parametrów cech na mapie (4.30). Ze względu na korelacje w macierzy kowariancji systemu (4.17) nowa obserwacja wpływa na niepewność wszystkich cech geometrycznych na mapie, nie tylko tych widocznych z aktualnej pozycji robota. Należy zwrócić uwagę, że nieprawidłowe dopasowanie obserwacji prognozowanych na podstawie mapy i obserwacji rzeczywistych może doprowadzić do nieprawidłowego działania filtru Kalmana, tj. zbyt optymistycznych estymat zarówno pozycji robota, jak i parametrów cech geometrycznych. Po predykcji i estymacji EKF bieżący wektor stanu i macierz kowariancji reprezentują parametry poszukiwanej funkcji rozkładu gęstości prawdopodobieństwa a posteriori dla pozycji robota i pozycji wszystkich znanych cech geometrycznych. 157

Cechy wyodrębnione z danych sensorycznych, które nie zostały dopasowane do żadnej z prognozowanych obserwacji, umieszczane są na mapie otoczenia i rozszerzają tym samym wektor stanu. Procedura dołączenia do mapy nowej cechy na podstawie i-tej obserwacji opisana jest równaniami: ˆx Fn+1(k) = g(ˆx (k k),z i(k) ), (4.31) C Fn+1(k) = J gr C R(k k) J T g R + J gf C ri(k)j T g F, (4.32) C Fn+1R(k) = J gr C R(k), (4.33) C Fn+1F j(k) = J gr C RFj(k), (4.34) gdzie j = 1...n, g jest funkcją transformującą obserwowane cechy z układu robota w cechy opisane w globalnym układzie współrzędnych mapy, a J gr = g x R i J gf = ˆxR(k k) g zi z i są jakobianami tej funkcji, odpowiednio względem pozycji robota i parametrów i-tej obserwacji. Wyznaczana jest macierz kowariancji nowej cechy (4.32), jej korelacja z pozycją robota (4.33) oraz korelacje z n cechami znajdującymi się już na mapie (4.34). Złożoność obliczeniowa tego etapu jest liniowa w funkcji liczby cech na mapie i wynosi O(nl), gdzie l jest liczbą nowych cech, dodanych do mapy w danym kroku. Stan robota w chwili k-1 Predykcja stanu robota w chwili k Predykcja obserwacji w chwili k Mapa w chwili k-1 Odometria Dopasowanie cech Obserwacje w chwili k TAK Estymacja stanu cech i robota NIE Nowe cechy k k-1 k k-1 Stan robota w chwili k Mapa w chwili k stan systemu w chwili k Rysunek 4.1. Schemat blokowy systemu SLAM opartego na EKF Estymata stanu systemu wraz z jej niepewnością w chwili k w kolejnym kroku iteracyjnego algorytmu SLAM staje się stanem wyjściowym w chwili k + 1 (rys. 4.1). Formalizm EKF pozwala wyznaczać stan systemu i jego niepewność (czyli parametry rozkładu prawdopodobieństwa (4.15)) w sposób iteracyjny. Algorytm EKF nie musi działać ze stałym krokiem, a etapy predykcji i estymacji mogą być wykonywane niezależnie. Na przykład, jeżeli na danym obszarze środowiska nie obserwuje się cech geometrycznych, to może nastąpić po sobie kilka etapów predykcji, jeżeli natomiast w chwili k + 1 rejestruje się wiele obserwacji, to można dokonać ich integracji w sposób sekwencyjny, używając wielokrotnie wzorów (4.28 4.30). W metodzie opartej na filtracji Kalmana na bieżąco integruje 158

się dane nadchodzące z sensorów, w przeciwieństwie do niektórych innych, znanych z literatury metod rozwiązania problemu SLAM, działających off-line i wymagających dostępu do kompletnych zbiorów danych z k i u k [188, 304]. Udowodniono [90] ważne własności algorytmu EKF-SLAM dotyczące jego zbieżności: przy liczbie obserwacji dążącej do nieskończoności kowariancja dowolnej cechy osiąga kres dolny, zdeterminowany jedynie początkową niepewnością pozycji robota; gdy liczba obserwacji dąży do nieskończoności, cechy na mapie stochastycznej stają się całkowicie skorelowane. Teoretyczne dowody zbieżności nie istnieją jak dotąd dla losowych metod rozwiązywania on-line problemu SLAM, takich jak filtracja cząsteczkowa [84, 209]. Należy jednak zwrócić uwagę, że zbieżność EKF-SLAM została wykazana dla idealnego przypadku liniowych obserwacji i znanych skojarzeń obserwacja cecha, co zmniejsza praktyczne znaczenie tego wyniku. Podczas użytkowania rzeczywistych systemów opartych na EKF-SLAM zaobserwowano wiele problemów prowadzących do pogorszenia dokładności estymaty pozycji robota i jakości mapy. Część z nich ma źródło w założeniach o postaci rozkładów prawdopodobieństwa, inne wynikają z niespełnienia w rzeczywistych systemach założeń dotyczących samego EKF. Jedno z najczęściej niespełnionych w praktyce założeń dotyczy wykorzystania zlinearyzowanych modeli robota i obserwacji. Linearyzacja daje dobre wyniki, gdy modelowane procesy są w przybliżeniu liniowe, a estymata stanu, w której otoczeniu dokonywana jest linearyzacja, jest dokładna [31]. W rzeczywistości funkcje opisujące zarówno pomiar, jak i ruch robota mają silnie nieliniowe składniki związane z orientacją, a estymata stanu otrzymana w wyniku predykcji na podstawie danych z odometrii może bardzo odbiegać od stanu rzeczywistego (por. p. 2.2). Najpoważniejszą konsekwencją niedokładnej predykcji stanu robota są nieprawidłowe połączenia prognozowanych i rzeczywistych obserwacji. Gdy dopasowanie jest błędne, EKF nieprawidłowo aktualizuje stan systemu. Jeżeli wskutek błędnej predykcji obserwacja nie będzie dopasowana do żadnej cechy geometrycznej na mapie, to dodawana jest nowa, w rzeczywistości nieistniejąca cecha. Ponieważ obserwacje i cechy są dopasowywane za pomocą mechanizmu zewnętrznego wobec filtru Kalmana (odległość Mahalanobisa, testy geometryczne), niepewność dopasowania, będąca formą niepewności co do przedmiotu pomiaru, nie jest uwzględniana i reprezentowana na mapie stochastycznej. Cechy geometryczne z założenia są zmiennymi losowymi o rozkładzie normalnym, a mapa stochastyczna reprezentuje pojedynczą hipotezę dotyczącą pozycji każdej z cech. Brak reprezentacji innych możliwych hipotez sprawia, że skutki błędnego dopasowania nie mogą być skorygowane na podstawie kolejnych prawidłowo dopasowanych obserwacji. Błędne połączenia powodują odchylenie estymaty stanu od stanu rzeczywistego (dywergencję filtru [31]) i utrudniają dopasowanie kolejnych obserwacji, co stopniowo prowadzi do nieprawidłowego działania EKF-SLAM. Także stan robota jest zmienną losową o rozkładzie Gaussa, która reprezentuje pojedynczą hipotezę dotyczącą pozycji. W rezultacie standardowy EKF-SLAM nie ma możliwości reprezentowania niejednoznacznej pozycji robota, która może być spowodowana np. symetrią otoczenia [25]. W większości znanych z literatury rozwiązań problemu SLAM dopasowanie między obserwacjami a mapą dokonywane jest na zasadzie najbliższego sąsiedztwa 1. Zaletami tej strategii wyboru są: prostota implementacyjna oraz efektywność obliczeniowa uwarunkowana 1 W literaturze metoda ta jest niekiedy określana mianem Nearest Neighbour Filter. 159

złożonością obliczeniową O(nm). Zasada najbliższego sąsiedztwa sprawdza się jednak jedynie wówczas, gdy prawdopodobieństwo prawidłowego dopasowania jest istotnie większe niż prawdopodobieństwo dopasowania błędnego [209], co zachodzi wówczas, gdy niepewność prognozowanej pozycji robota, niepewność pozycji cech geometrycznych oraz niepewność pomiaru są mniejsze niż odległości (separacje) między cechami na mapie. Powyższe uwarunkowania zazwyczaj nie są spełnione, gdy używa się mniej precyzyjnych sensorów (np. monookularowego systemu wizyjnego [72]) i w warunkach występowania istotnej niepewności co do przedmiotu pomiaru (np. w przypadku użycia sonarów), ale także wówczas, gdy niepewność pozycji robota jest duża, np. po przebyciu znacznego dystansu (problem zamykania pętli), lub gdy występują katastrofalne błędy odometrii (por. p. 2.2). W literaturze podaje się także opis kilku bardziej zaawansowanych i złożonych strategii dopasowywania danych podczas rozwiązywania problemu SLAM. Autorzy [221] pokazali, że skuteczność dopasowania można poprawić przez łączne rozpatrywanie dopasowania wszystkich obserwacji dokonanych w danym kroku. Oparty na podobnej koncepcji algorytm dopasowywania obserwacji do cech zaprezentowano także w rozprawie [28]. Tego rodzaju algorytmy pozwalają uzyskać lepsze wyniki dopasowywania niż standardowa strategia najbliższego sąsiedztwa, jednak w każdym kroku jest wybierany jeden zestaw dopasowanych cech, a wszystkie pozostałe możliwości są ignorowane, co nie pozwala na ewentualną rewizję połączeń obserwacja cecha na podstawie danych uzyskanych w kolejnych krokach. Rozwiązaniem tego problemu jest jednoczesne śledzenie wielu hipotez (ang. Multiple Hypotheses Tracking MHT), które w przypadku EKF-SLAM przyjmuje formę zbliżoną do znanego z radiolokacji jednoczesnego śledzenia wielu obiektów z użyciem filtru Kalmana [31]. Metodę MHT zastosowano z powodzeniem w systemach samolokalizacji opartych na EKF i wektorowej reprezentacji otoczenia [23, 141]. Istota tej metody polega na odłożeniu w czasie ostatecznej decyzji o połączeniu obserwacji i cechy z mapy. Jeżeli istnieje wiele cech, które zostały dopasowane do danej obserwacji w sensie warunku (3.84), to tworzonych jest wiele hipotez wyjaśniających pochodzenie tej obserwacji. Hipotezy te podlegają propagacji przez kolejne iteracje filtru Kalmana w nadziei, że nowe informacje (pomiary) pozwolą rozstrzygnąć konflikt. By uniknąć eksplozji liczby hipotez w kolejnych krokach procedury samolokalizacji, do każdej hipotezy przypisywane jest prawdopodobieństwo jej prawdziwości, a do usuwania hipotez najmniej prawdopodobnych używa się metod heurystycznych opartych na ograniczeniach geometrycznych [23]. Generowanie i śledzenie wielu hipotez w przypadku EKF-SLAM jest trudne i bardzo kosztowne obliczeniowo, gdyż (inaczej niż w metodzie samolokalizacji EKF) każda z tych hipotez jest odrębną mapą, opisującą nie tylko pozycję robota, lecz także pozycje wszystkich cech geometrycznych. Mapa stochastyczna reprezentowana jest więc przez mieszaninę rozkładów gaussowskich (ang. mixture-of-gaussian), przy czym każda hipoteza jest mapą o ustalonych, dopasowanych parach obserwacja cecha, opisującą funkcję rozkładu gęstości prawdopodobieństwa postaci (4.15). Każda hipoteza reprezentuje niepewność wynikającą z modelu obserwacji (sensorów) i modelu robota (por. wzór 4.14), natomiast niepewność dopasowania, wynikająca z niepewności co do przedmiotu pomiaru, reprezentowana jest w sposób dyskretny, przez zbiór hipotez. Uwzględnienie wielu hipotez jest łatwiejsze w tych metodach rozwiązania problemu SLAM, które dopuszczają reprezentację (4.9) za pomocą arbitralnego (wielomodalnego) rozkładu prawdopodobieństwa. Metodą taką jest filtracja cząsteczkowa, w której zarówno niepewność pochodząca od modeli obserwacji i robota, jak i niepewność co do skojarzeń obserwacja cecha reprezentowana jest przez skończony zbiór cząsteczek, będących próbkami wylosowanymi z rozkładu (4.9) [210]. Od- 160

bywa się to jednak kosztem wprowadzenia czynnika losowego i utraty deterministycznego, zamkniętego charakteru rozwiązania (4.14), które zapewnia EKF-SLAM. Ograniczeniem stosowania EKF-SLAM jest także złożoność obliczeniowa tej metody. Jest ona uwarunkowana złożonością etapu estymacji, proporcjonalną do kwadratu liczby cech geometrycznych zgromadzonych na mapie stochastycznej. Kwadratowa złożoność obliczeniowa etapu estymacji EKF-SLAM jest konsekwencją przyjętego założenia, że funkcja (4.9) reprezentowana jest jako wielowymiarowy rozkład normalny (4.15). Rozkład ten jest określony wektorem stanu (4.16) i macierzą kowariancji (4.17) o wymiarze 2n + 3 2n + 3, która zawiera korelacje pomiędzy wszystkimi zmiennymi stanu. W celu aktualizacji macierzy kowariancji na podstawie nowych obserwacji (4.30) należy obliczyć wartości wszystkich korelacji w tej macierzy, co wymaga liczby operacji proporcjonalnej do n 2. Z tego powodu estymacja wektora stanu w czasie rzeczywistym jest trudna, a dla bardzo dużej liczby cech niemożliwa z uwagi na ograniczoną moc obliczeniową pokładowych systemów komputerowych robotów mobilnych. Warto zauważyć, że problem złożoności obliczeniowej nie dotyczy metody samolokalizacji ze znaną mapą opartej na EKF, ponieważ w tym wypadku wektor stanu zawiera jedynie pozycję robota i nie ulega rozbudowie. Rozwiązaniem problemu złożoności obliczeniowej EKF-SLAM nie może być ignorowanie korelacji między cechami, gdyż jak wykazano w pracy [70], podejście takie szybko prowadzi do optymistycznych (zaniżonych) oszacowań kowariancji cech i pozycji robota, a w rezultacie do niespójnej w sensie geometrycznym mapy. Wiele prac badawczych dotyczących EKF-SLAM koncentruje się na zwiększeniu efektywności obliczeniowej tej metody przez stosowanie algorytmów suboptymalnych, które stanowią aproksymację podstawowego algorytmu EKF, ale dzięki mniejszej złożoności obliczeniowej pozwalają na budowę map znacznie większych obszarów [103, 141, 183]. W literaturze są też opisywane algorytmy bardziej efektywnego obliczeniowo rozwiązania problemu SLAM, które zachowują optymalne własności podstawowego algorytmu EKF-SLAM [83, 122, 163]. Algorytmy te aktualizują pełną macierz kowariancji (4.17), lecz integrują obserwacje na mapie lokalnej i opóźniają integrację nowych informacji z mapą globalną do momentu, gdy robot opuszcza obszar objęty mapą lokalną. W tym przypadku wzrost efektywności obliczeniowej jest jednak mniejszy. 4.3. Realizacja algorytmu EKF-SLAM 4.3.1. Reprezentacja mapy wektorowej Mapa stochastyczna wykorzystywana w metodzie EKF-SLAM może reprezentować informacje o dowolnych cechach otoczenia wyodrębnionych z danych pomiarowych. Rodzaj cech w oczywisty sposób zależy od rodzaju sensorów zewnętrznych, w które wyposażony jest robot, oraz typu środowiska, w którym ten robot działa. Z obrazów uzyskiwanych z pasywnych sensorów wizyjnych najczęściej są wyodrębniane krawędzie pionowe, reprezentowane następnie jako punktowe cechy na mapie 2D [23, 72], lub krawędzie pionowe i poziome łączone w szkieletową mapę 3D [200]. Odmienne podejście do percepcji wizyjnej wykorzystywanej w metodzie EKF-SLAM polega na analizie lokalnych właściwości obrazu w celu wykrycia tzw. wybitnych cech (ang. salient features), które nie mają jednak konkretnej interpretacji geometrycznej w obserwowanym otoczeniu, a reprezentowane są na mapie jako punkty [83]. Z danych dostarczanych przez skanery laserowe 2D wyodrębniane są cechy punktowe [90, 199, 209, 219] lub odcinki [23, 72, 141]. 161

Bardzo istotny jest wybór właściwej reprezentacji matematycznej cech na mapie stochastycznej. Jest to kwestia ważna przede wszystkim dla zapewnienia prawidłowej reprezentacji i propagacji niepewności. Reprezentacja cech nie powinna mieć osobliwości, gdyż w pobliżu osobliwości wartości elementów macierzy kowariancji dążą do nieskończoności [289]. Wybrana reprezentacja powinna być minimalna, tj. nie powinna się charakteryzować nadmierną liczbą parametrów, ponieważ występowanie parametrów, których wartości są nieokreślone lub zależne od siebie, może sprawiać, że macierze kowariancji będą osobliwe lub będą zawierać elementy o wartościach dążących do nieskończoności [72]. Reprezentacja niepewności cechy powinna być niezmiennicza względem zmiany układu współrzędnych, tj. wartości niepewności poszczególnych parametrów cechy nie powinny zależeć od położenia cechy względem początku układu współrzędnych. Mapa stochastyczna zaproponowana po raz pierwszy w pracy [290] składała się z cech punktowych. Punktowe cechy (nazywane też landmarkami) stosowane są w wielu implementacjach EKF-SLAM. W środowisku naturalnym reprezentują one takie obiekty jak pnie drzew [90], a wewnątrz pomieszczeń narożniki [23, 199]. Cechy punktowe, reprezentowane jako pary współrzędnych f = [x y] T, spełniają wszystkie wyżej wymienione postulaty dotyczące matematycznej reprezentacji cech. Ich wadą jest jednak bezwymiarowość (w sensie geometrycznym), która ogranicza możliwość rozróżniania takich cech, gdyż jedynym parametrem, który można porównywać, jest położenie cechy. Mapa składająca się z cech punktowych nie spełnia także kryteriów reprezentacji uniwersalnej z punktu widzenia potrzeb systemu nawigacji robota (por. p. 3.1), jest to mapa wykorzystywana wyłącznie do samolokalizacji. Bardziej efektywnym sposobem opisu środowisk zbudowanych przez człowieka, a w szczególności wnętrz pomieszczeń, na podstawie danych ze skanera laserowego jest mapa wektorowa złożona z cech-odcinków. Ten sposób reprezentacji informacji przestrzennej został szczegółowo omówiony w podrozdziale 3.3. Zaletą mapy wektorowej z punktu widzenia implementacji metody EKF-SLAM jest możliwość sformułowania dodatkowych ograniczeń wykorzystywanych na etapie dopasowywania cech. Ograniczenia te mają charakter relacji geometrycznych związanych z długością odcinka (por. rys. 3.21). Problemem jest natomiast znalezienie takiej matematycznej reprezentacji odcinka lub jego linii wspierającej, która spełnia sformułowane powyżej postulaty. Ponieważ przedstawiony w niniejszym podrozdziale system SLAM przeznaczony jest dla robota TRC Labmate wyposażonego w skaner laserowy LMS 200 i działającego w środowisku zbudowanym przez człowieka, jako cechy wybrano odcinki. W rozważaniach dotyczących metod tworzenia mapy wektorowej (p. 3.3) linie wspierające odcinków reprezentowano za pomocą równania prostej w postaci normalnej (3.63). Jest to wygodna reprezentacja dla procedur wyodrębniania odcinków z danych pomiarowych, jednak niepewność parametru ρ linii wspierającej nie jest niezmiennicza względem zmiany układu współrzędnych. Znajduje to odzwierciedlenie w jakobianach (3.82) i (3.83) wykorzystywanych podczas propagacji niepewności linii z układu robota do układu globalnego. Wyrażenia tych jakobianów zależą od położenia robota w układzie globalnym. Podczas transformacji danej prostej w prostą opisaną w nowym układzie współrzędnych, którego początek jest odległy od przekształcanej prostej, nawet niewielka niepewność orientacji φ powoduje dużą niepewność odległości ρ po transformacji. 162

Nowe podejście do reprezentacji niepewności geometrycznej, nazwane modelem symetrii i zakłóceń (ang. Symmetries and Perturbations model SPmodel), zaproponował Tardós [302]. Podejście to oparte jest na zastosowaniu teorii symetrii do specyfikacji informacji opisujących różne rodzaje cech oraz wykorzystaniu lokalnej reprezentacji niepewności geometrycznej. W pracy [71] Tardós i współautorzy zaproponowali użycie formalizmu SPmodel do opisu cech na potrzeby systemu EKF-SLAM opracowali mapę stochastyczną nazwaną SPmap. Koncepcja SPmodel umożliwia re- prezentację dowolnych cech geometrycznych wraz z ich niepewnością bez osobliwości, w sposób minimalistyczny oraz niezmienniczy względem zmiany układu współrzędnych. Ponieważ SPmodel może być użyty do reprezentacji odcinków na geometrycznej mapie 2D, zdecydowano się wykorzystać go w implementacji systemu EKF-SLAM. Za pomocą SPmodel można reprezentować w ujednolicony sposób różne cechy (np. odcinki i punkty), które mogą współistnieć na jednej mapie SPmap [72]. Ponieważ w opisywanej implementacji Y W Y X WF F X d F X Rysunek 4.2. Rzeczywista i estymowana pozycja cechy SPmodel EKF-SLAM w obecnej postaci wykorzystuje się tylko dane ze skanera laserowego 2D, użyto wyłącznie odcinków. Pełną specyfikację SPmodel/SPmap podano w monografii [72]. By umożliwić matematyczny opis dokonanej implementacji systemu SLAM, poniżej przedstawiono podstawowe idee oraz konwencję notacji cech SPmodel, a także zdefiniowano mapę wektorową w reprezentacji SPmap. W reprezentacji SPmodel z każdą cechą geometryczną związany jest lokalny układ współrzędnych F, którego początek leży w punkcie zdefiniowanym przez położenie tej cechy. Dla przykładu, początek układu lokalnego przypisanego do cechy będącej punktem leży w tym punkcie, natomiast początek układu związanego z cechą będącą linią prostą znajduje się na tej prostej, przy czym prosta pokrywa się z osią 0X tego układu i wyznacza jego orientację względem układu globalnego. Pozycja cechy jest traktowana jako transformacja (złożona z translacji i rotacji) określonego układu globalnego W w lokalny układ współrzędnych cechy: x WF = [x y φ] T. (4.35) Operacje, którym są poddawane relacje przestrzenne w reprezentacji SPmodel, zdefiniowane są analogicznie jak w artykule [289]. Operacja złożenia (ang. compounding) dwóch transformacji: x WB = x WA x AB, pozwala wyznaczyć pozycję układu B w układzie globalnym W, gdy znana jest pozycja układu B w układzie A i układu A w układzie globalnym. Operacja inwersji x AW = x WA pozwala uzyskać transformację odwrotną. Ujednoliconą reprezentację różnych typów cech geometrycznych uzyskuje się dzięki koncepcji symetrii. Symetrią cechy danego typu nazywana jest transformacja niezmiennicza tej cechy. Symetria cechy reprezentowana jest przez zero-jedynkową macierz, nazywaną macierzą łączącą (ang. binding matrix), której zadaniem jest wybieranie z wektora pozycji cechy (4.35) współrzędnych podlegających transformacji. X WF Y F X 163

W reprezentacji SPmodel niepewność pozycji cechy wyrażona jest lokalnie, względem układu współrzędnych danej cechy (rys. 4.2). Estymowana pozycja cechy geometrycznej reprezentowana jest przez wektor x WF postaci (4.35). Błąd estymacji jest natomiast reprezentowany w układzie lokalnym przez wektor różnicy pozycji d F (ang. differential location vector). Rzeczywista pozycja cechy dana jest więc wzorem: x WF = ˆx WF d F = ˆx WF B T Fp F, (4.36) gdzie p F jest wektorem zakłócenia (ang. perturbation vector), zdefiniowanym jako p F = B F d F. (4.37) Tak zdefiniowany wektor zakłócenia pozwala wykluczyć z obliczeń dotyczących niepewności te współrzędne cechy, które odpowiadają jej stopniom swobody, a zatem ich niepewność nie jest określona. Macierz kowariancji wektora zakłóceń można ogólnie zapisać jako C F = E [ (p F ˆp F )(p F ˆp F ) T], ˆp F = E[p F ]. (4.38) Dowolna cecha A, a także robot, reprezentowana jest w notacji SPmodel jako czwórka: L WA = (ˆx WA, ˆp A,C A,B A ). (4.39) Odcinki stanowiące cechy geometryczne na mapie wektorowej użytej w przedstawianej implementacji EKF-SLAM reprezentowane są przez pozycję układu współrzędnych związanego z linią wspierającą danego odcinka oraz dodatkowe parametry opisujące środek i długość odcinka (por. p. 3.3). granice obszaru niepewnoœci Y Y X WE d x d x E y E l E E X W X linia wspieraj¹ca Rysunek 4.3. Reprezentacja odcinka na mapie SPmap Cecha-odcinek leży na nieskończonej linii wspierającej. Do cechy przypisany jest lokalny układ współrzędnych E, którego początek zaczepiony jest w środku odcinka, a oś 0X pokrywa się z linią wspierającą. Ponieważ linia wspierająca cechy może być opisana układem E zaczepionym w dowolnym jej punkcie, to translacja wzdłuż kierunku tej prostej stanowi stopień swobody cechy. Stąd niepewność linii wspierającej przejawia się jedynie jako translacja 164

d y i rotacja d φ względem punktu, w którym umiejscowiony jest układ lokalny (rys. 4.3). Pozycja linii wspierającej cechy geometrycznej zapisywana jest w konwencji SPmodel jako L WE = (ˆx WE, ˆp E,C E,B E ), (4.40) gdzie wektor zakłócenia, macierz wiążąca i macierz kowariancji mają postać: p E = [ [ ] [ ] ] T 0 1 0 σ 2 d y d φ, BE =, C 0 0 1 E = ye σ yφe σ φye σφ 2. (4.41) E Mapa wektorowa w reprezentacji SPmap jest czwórką M W = (ˆx W, ˆp W,C W,B W ), (4.42) przy czym wektor stanu złożony jest z estymowanych pozycji cech geometrycznych i robota. Ogólna postać macierzy kowariancji C W = E[(p W ˆp W )(p W ˆp W ) T ] dana jest wzorem (4.17). Macierz łącząca mapy wektorowej jest macierzą blokowo-diagonalną złożoną z macierzy łączących wszystkich cech geometrycznych oraz macierzy łączącej robota: B W = diag(b R,B F1,B F2,...B Fn ). Pozycje wszystkich elementów na mapie wektorowej SPmap, zarówno cech-odcinków jak i robota, wyrażone są względem globalnego układu współrzędnych W. Układ ten zaczepiony jest w początkowym (startowym) położeniu robota, a kierunek jego osi 0X pokrywa się z początkową orientacją robota. Dzięki temu nie istnieje początkowa niepewność pozycji robota, która jak wykazano w artykule [74] może prowadzić do wzrostu błędów linearyzacji EKF. 4.3.2. Ekstrakcja cech Podczas akwizycji danych ze skanera dokonywana jest korekcja błędów systematycznych pomiaru odległości metodą opisaną w podrozdziale 2.4.3. Niepewność pomiaru wyrażono macierzą diagonalną C M (3.56). Cały skan, traktowany jako zbiór pomiarów odległości, jest dzielony na podzbiory reprezentujące odrębne obiekty za pomocą przedstawionej w podrozdziale 3.3.3 procedury RANGECLUSTERING (algorytm 3.2). Wynikiem grupowania są zbiory pomiarów, które podlegają konwersji na punkty wyrażone we współrzędnych kartezjańskich względem układu współrzędnych robota. Macierz kowariancji punktu C p (3.57) wyznaczana jest przez aproksymację wzorem (3.58). Procedura segmentacji służy do wyodrębniania ze zbiorów punktów otrzymanych na poprzednim etapie mniejszych grup, dogodnych do aproksymacji odcinkami. Segmentacja odbywa się za pomocą algorytmu Iterative End Point Fit, opisanego w podrozdziale 3.3.4. Dla każdego z wyodrębnionych odcinków wyznaczane są parametry równania linii wspierającej, która zawiera ten odcinek. Parametry równania postaci (3.63) estymowane są za pomocą procedury ROBUSTLINEFIT (algorytm 3.3), w której wykorzystuje się metodę najmniejszych kwadratów (3.65, 3.66). Na mapie SPmap pozycja cechy (4.40) jest reprezentowana przez wektor określający położenie i orientację związanego z nią układu współrzędnych względem układu robota: L RE = (ˆx RE, ˆp E,C E,B E ). (4.43) 165

Zgodnie ze wzorem (4.35) wektor opisujący położenie i orientację układu cechy ma postać: x RE = [x e y e φ e ] T. (4.44) Składniki wektora (4.44) obliczane są na podstawie parametrów prostej (3.63) oraz współrzędnych punktów końcowych odcinka. W reprezentacji SPmodel niepewność pozycji cechy wyrażana jest lokalnie, za pomocą wektora zakłóceń oraz jego macierzy kowariancji (4.41). Równanie pomiaru, które wiąże wektory zakłóceń obserwacji (punktu otrzymanego z pomiaru) i cechy (4.43), ma postać: f m (p E,p P ) = B EP x EP, (4.45) a do wyznaczenia macierzy kowariancji C E wykorzystywany jest, podobnie jak w pracy [72], rozszerzony filtr informacyjny (ang. Extended Information Filter EIF), który w przeciwieństwie do EKF operuje na z góry znanej liczbie obserwacji [30, 218]. W stochastycznej reprezentacji SPmap cechy opisane są ich nieskończonymi liniami wspierającymi. Wykorzystanie formalizmu SPmodel/SPmap jest dogodne z punktu widzenia metody EKF-SLAM, jednak jest niewystarczające podczas dopasowywania obserwacji rzeczywistych i prognozowanych, podczas którego istotną rolę odgrywają testy geometryczne (por. p. 4.2). Reprezentacja cechy w postaci (4.40) jest więc uzupełniana długością odcinka l E, obliczoną na podstawie wcześniej ustalonych współrzędnych jego końców. Niepewność długości odcinka nie jest określana. 4.3.3. Wykorzystanie reprezentacji SPmap w algorytmie EKF-SLAM Etap predykcji Operacja prognozowania stanu robota w chwili k wykonywana jest na podstawie stanu w chwili k 1 oraz informacji uzyskanych z odometrii. Zgodnie z przyjętym modelem odometrii (por. p. 2.2), uwzględniającym wyłącznie błędy przypadkowe, niepewność przemieszczenia opisana jest rozkładem normalnym. Prognozowany wektor stanu SPmap ma więc postać: ˆx W (k k 1) = ˆx WR(k k 1) ˆx WF1(k k 1) ˆx WF2(k k 1). ˆx WFn(k k 1) = ˆx WR(k 1 k 1) ˆx Rk 1 R k ˆx WF1(k 1 k 1) ˆx WF2(k 1 k 1). ˆx WFn(k 1 k 1), (4.46) gdzie ˆx Rk 1 R k jest otrzymaną z odometrii estymatą przemieszczenia. Ponieważ w reprezentacji SPmodel błędy są wyrażane przez wektor zakłóceń, predykcja z użyciem rozszerzonego filtru Kalmana dotyczy wektora zakłóceń SPmap i jego macierzy kowariancji: ˆp W (k k 1) = F (k)ˆp W (k 1 k 1), (4.47) C W (k k 1) = F (k)c W (k 1 k 1) FT (k) + G (k)c Rk 1 R k G T (k), (4.48) gdzie C Rk 1 R k jest macierzą kowariancji reprezentującą niepewność przemieszczenia, a macierze F i G mają następującą postać: [ JRk R F (k) = 0 ] [ ] k 1 3 n I3 1, G 0 n 3 I (k) =, (4.49) n n 0 n 1 166

gdzie J Rk R k 1 jest zdefiniowanym w pracy [72] jakobianem transformacji układów współrzędnych. Oszacowanie wektora zakłóceń SPmap i jego macierzy kowariancji w chwili k na podstawie informacji uzyskanych do chwili k 1 i modelu systemu przyjmuje ostatecznie postać: ˆp W (k k 1) = J Rk R k 1 ˆdR(k 1 k 1) ˆp F1 (k 1 k 1) ˆp F2 (k 1 k 1). ˆp Fn(k 1 k 1), (4.50) C W (k k 1) = J Rk R k 1 C R(k 1 k 1) J T R k R k 1 + J Rk R +C k 1 C RF1 (k 1 k 1)... J Rk R k 1 C RFn(k 1 k 1) Rk 1 R k = J Rk R k 1 C F1 R(k 1 k 1) C F1 (k 1 k 1)... C F1 F n(k 1 k 1)......... J Rk R k 1 C FnR(k 1 k 1) C FnF1 (k 1 k 1)... C Fn(k 1 k 1) (4.51) Etap estymacji Estymacja stanu za pomocą EKF na podstawie uprzednio dokonanej predykcji i nowych obserwacji wymaga sformułowania równania pomiaru, wiążącego estymowany stan z obserwacjami. W przypadku algorytmu EKF-SLAM, w którym wykorzystuje się mapę stochastyczną, związek między stanem a obserwacjami przybiera formę ograniczeń geometrycznych wiążących cechy na mapie i cechy uzyskane z pomiarów. Dokonane na poprzednim etapie oszacowanie wektora zakłóceń SPmap i jego macierzy kowariancji jest korygowane za pomocą EKF w taki sposób, by spełnić te ograniczenia. Z wykorzystaniem reprezentacji SPmodel ograniczenie geometryczne można wyrazić przez koincydencję (z uwzględnieniem symetrii) cechy zapisanej na mapie, wyrażonej względem globalnego układu współrzędnych: L WF = (ˆx WF, ˆp F,C F,B F ), (4.52) oraz i-tej cechy otrzymanej z obserwacji, wyrażonej względem układu robota: L REi = (ˆx REi, ˆp Ei,C Ei,B E ). (4.53) Równanie pomiaru dla i-tej obserwacji jest wówczas dane implicite: h i (p W,p Ei ) = B FE x FEi = 0, (4.54) gdzie B FE jest macierzą łączącą dopasowania cechy F z mapy do obserwacji E. Dla wykorzystanych cech, będących odcinkami, równanie (4.54) określa warunek współliniowości ich linii wspierających. Linearyzacja (4.54) dokonywana jest przez rozwinięcie w szereg Taylora: 167

v i = h i (ˆp W, ˆp E ) = B FEˆx FEi, (4.55) H i = h i p W = [ H R i 0... 0 H E i 0... 0 ], (4.56) ˆp W,ˆp E H R i = B FE J 2 J ER, (4.57) H E i = B FE J 1 B T F, (4.58) G i = h i = B FE J 2 B T p E, (4.59) E ˆp W,ˆp E gdzie J ER jest jakobianem transformacji układu cechy w układ robota, a J 1 i J 2 są jakobianami operacji złożenia dwóch transformacji, odpowiednio względem pierwszej i drugiej transformacji, zdefiniowanymi przez Smitha i Cheesemana [289]. Wektor v i jest błędem predykcji i-tej obserwacji (innowacją). Wyrażenia opisujące macierze H i i G i w postaci rozwiniętej, dogodnej do implementacji, podano w pracy [323]. Estymacja stanu musi być poprzedzona znalezieniem właściwych skojarzeń między obserwacjami a cechami znajdującymi się na mapie. Ponieważ SPmap reprezentuje cechy-odcinki w postaci ich linii wspierających, to w celu dopasowania odcinków z mapy do odcinków obserwowanych konieczne jest zastosowanie testów geometrycznych opisanych w podrozdziale 3.3. Odcinki dopasowane wstępnie metodami geometrycznymi poddaje się testowi statystycznemu opartemu na odległości Mahalanobisa (3.84). Uwzględniając wzory (4.55 4.59), test ten można zapisać jako: d 2 = v T (k) i (H (k)i C W (k+1 k) i 1 H T (k) i + G (k)i C Ei(k)G T (k) i ) 1 v (k)i χ 2 r,α, (4.60) gdzie H (k)i = H i, G (k)i = G i w chwili k, a C W (k k 1) i 1 jest aktualną macierzą kowariancji SPmap, uwzględniającą informacje uzyskane na podstawie integracji poprzednich i 1 obserwacji w chwili k (por. wzór (4.63)). Prawdopodobieństwo α wynosi 95%, a liczba stopni swobody r = rank(v i ). W chwili k dokonywanych jest n f niezależnych obserwacji. Obserwacje dopasowane do cech znajdujących się na mapie są integrowane z użyciem EKF (4.28 4.30). Wektor zakłóceń i macierz kowariancji SPmap mogą być estymowane w jednym kroku na podstawie wektora o wymiarach 1 2m utworzonego z wektorów innowacji (4.55) m dopasowanych obserwacji oraz blokowo-diagonalnej macierzy kowariancji o wymiarach 2m 2m zawierającej macierze C Ei wszystkich dopasowanych cech [323]. Jednak ze względu na niezależność obserwacji można użyć iteracyjnego sformułowania etapu estymacji EKF [30], pozwalającego na stopniową integrację kolejnych obserwacji L REi (i = 1...m). Zaletą iteracyjnej wersji EKF jest to, że po zintegrowaniu każdej obserwacji obliczana jest nowa estymata stanu, dzięki czemu podczas integracji kolejnych obserwacji zmniejszają się błędy linearyzacji (4.56), (4.59), gdyż jest ona dokonywana zawsze w otoczeniu najbardziej aktualnej estymaty stanu. Etap estymacji iteracyjnej wersji EKF dla wektora zakłóceń SPmap opisany jest następującymi równaniami: K (k)i = C W (k) i 1 H T (k) i (H (k)i C W (k) i 1 H T (k) i + G (k)i C E(k)i G T (k) i ) 1, (4.61) p W (k) i = p W (k) i 1 + K (k)i v (k)i, (4.62) C W (k) i = (I K (k)i H (k)i )C W (k) i 1, (4.63) 168

gdzie p W (k) 0 = p W (k k 1), CW (k) 0 = C W (k k 1). Wektor innowacji v i ma wymiary 1 2, wymiary macierzy H i wynoszą 2 2n + 3, a wymiary macierzy G i to 2 2. W reprezentacji SPmap wektor pozycji cech i robota ˆx W jest bazą dla estymowanych wektorów zakłóceń ˆp W. By otrzymać zaktualizowaną pozycję elementu SPmap (cechy geometrycznej lub robota), należy uwzględnić aktualną wartość estymaty wektora zakłóceń tego elementu. Operacja ta nosi miano centrowania [72] i polega na dodaniu wektorów zakłóceń do odpowiednich wektorów pozycji. Wektory zakłóceń są następnie zerowane. Centrowanie SPmap wykonuje się jednocześnie na całym wektorze stanu, a operacja ta modyfikuje również elementy macierzy kowariancji C W. Podczas estymacji EKF centrowanie wektora stanu i zerowanie wektorów zakłóceń wykonuje się po integracji każdej kolejnej obserwacji (czyli m razy w każdym kroku algorytmu EKF), co pozwala dokonywać linearyzacji (4.56) i (4.59) w otoczeniu najlepszej, centrowanej (ˆp W = 0) estymaty. Rozbudowa mapy Obserwacje, które nie zostały dopasowane do żadnej cechy na mapie, są wprowadzane do SPmap jako nowe cechy i rozszerzają tym samym wektor stanu. Dla uproszczenia notacji założono, że mapa SPmap ma następującą, uogólnioną postać: ˆx W = [ ] ˆxWR, ˆp ˆx W = WM [ ˆdR ˆp M ], C W = [ CR C RM C T RM C M ], (4.64) a nowa cecha zapisana jest jako L RE = (ˆx RE, ˆp E,C E,B E ). W wyniku operacji dodania nowej cechy komponenty SPmap przyjmują postać: ˆx WR d R ˆd R ˆx W = ˆx WM ˆx WR ˆx RE, ˆp W = p M = p W F ˆp M B F J ERˆdR + ˆp E, (4.65) C R C RM C R J T C W ER BT E = C T RM C M C T RM JT ER BT E. (4.66) B F J ER C R B E J ER C RM B E J ER C R J T ER BT E + C E 4.3.4. Wyniki eksperymentów Wyniki jakościowe W celu sprawdzenia poprawności działania opisanej implementacji EKF-SLAM wykonano liczne eksperymenty. Użyto w nich robota mobilnego TRC Labmate (rys. 4.4A). Testy prowadzono w pomieszczeniu Laboratorium Robotów Mobilnych IAiII (rys. 4.4B) oraz w korytarzu pierwszego piętra budynku Wydziału Elektrycznego Politechniki Poznańskiej. Podczas każdego z eksperymentów zadaniem robota było przebycie określonej ścieżki zadanej przez operatora w postaci listy elementarnych ruchów (przesunięć i/lub obrotów). Robot poruszał się ze średnią prędkością liniową 0,2 m s. Zaplanowana (nominalna) ścieżka była śledzona przez robota na podstawie informacji z odometrii. Ponieważ program realizujący algorytm EKF-SLAM nie dokonuje korekcji ścieżki na podstawie estymowanej pozycji, robot nie zawsze precyzyjnie osiąga wyznaczone punkty węzłowe tej ścieżki. Jednak takie eksperymenty pozwalają porównać wyniki samolokalizacji za pomocą odometrii i metody 169

A B Rysunek 4.4. Robot TRC Labmate (A) i widok wnętrza Laboratorium Robotów Mobilnych (B) EKF-SLAM oraz pozwalają zapisywać surowe dane, które mogą być następnie wielokrotnie przetwarzane off-line, co umożliwia badanie wpływu parametrów procedur ekstrakcji cech, testów geometrycznych itp. [323] oraz testowanie kolejnych modyfikacji metody SLAM. Wszystkie prezentowane w niniejszym podrozdziale eksperymenty wykonano przy takich samych wartościach parametrów procedur ekstrakcji cech oraz geometrycznych testów dopasowania cech w metodzie EKF-SLAM. Wartości te dobrano na podstawie wcześniejszych prób. Poniżej zaprezentowano wyniki dwóch eksperymentów, wykonanych w różnych środowiskach. W eksperymencie pierwszym robot poruszał się po ścieżce w kształcie prostokąta o łącznej długości 10 m, którą wytyczono w pomieszczeniu laboratorium. Możliwości percepcyjne skanera laserowego 2D zamontowanego na robocie Labmate ograniczają się do płaszczyzny na wysokości 0,25 m ponad podłożem. Umieszczenie skanera na tej wysokości pozwala używać go nie tylko jako sensora dostarczającego dane do systemu SLAM, lecz także jako źródła informacji do algorytmu omijania przeszkód [264]. Jednak w typowym środowisku zbudowanym przez człowieka (biuro, szpital lub inny obiekt użyteczności publicznej) w polu widzenia tak umieszczonego skanera znajdują się często nogi stołów i krzeseł lub inne obiekty, których nie można dobrze aproksymować odcinkami oraz które mogą być przemieszczone, co może stwarzać problemy podczas tworzenia mapy wektorowej. W związku z tym w prezentowanych eksperymentach otoczenie robota w laboratorium zostało częściowo przystosowane do jego możliwości percepcyjnych przesłonięto niektóre obszary płytami styropianowymi. Ustawiono także dodatkowe obiekty o regularnych kształtach (por. rys. 4.4B). Na rysunkach 4.5A 4.5E przedstawiono widoki mapy wektorowej po aktualizacji w wybranych fazach eksperymentu. Mapa wektorowa na rys. 4.5F (powiększona) jest końcowym rezultatem działania metody EKF-SLAM. Na rysunkach linia ciągła z punktami obrazuje zaplanowaną (nominalną) ścieżkę robota, a linia przerywana ścieżkę powstałą przez połączenie kolejnych położeń robota estymowanych metodą EKF-SLAM (jest to więc estymata ścieżki rzeczywistej). Punkty ścieżki nominalnej, w których robot wykonywał procedurę SLAM, zaznaczono na rysunku małymi okręgami. 170

A B C D E F B C B D C A Rysunek 4.5. Proces tworzenia mapy podczas eksperymentu w laboratorium 171

Jak widać na rys. 4.5, w wyniku zastosowania metody EKF-SLAM, zaimplementowanej w sposób opisany w niniejszym podrozdziale, uzyskano stosunkowo dokładną mapę wektorową pomieszczenia laboratorium, mimo że dysponowano jedynie danymi ze skanera laserowego i enkoderów kół robota. Jednocześnie uzyskano estymatę rzeczywistej ścieżki przebytej przez robota. Jak widać na rys. 4.5F, odzwierciedla ona błędy charakterystyczne dla robota z układem jezdnym differential drive, a związane głównie ze zmianą orientacji. Niepewność położenia robota estymowaną metodą EKF-SLAM przedstawiono jako elipsę (por. p. 2.2). Na rysunku 4.5 elipsy są mało widoczne, gdyż estymowana niepewność położenia jest bardzo mała. Na otrzymanej mapie wektorowej wiele krawędzi obiektów reprezentowanych jest przez zwielokrotnione odcinki lub ich wiązki (np. w miejscach oznaczonych B i C), co świadczy o nie w pełni skutecznym dopasowywaniu cech w metodzie EKF-SLAM. Ponieważ dopasowanie dokonywane jest w przestrzeni parametrów cech za pomocą testu opartego na kwadracie odległości Mahalanobisa, zgodnie z zasadą najbliższego sąsiedztwa, to decydujące znaczenie dla jego skuteczności ma prawidłowa predykcja pozycji robota i jej niepewności (4.51). Biorąc pod uwagę ostateczny kształt uzyskanej mapy, otrzymane estymaty niepewności pozycji należy uznać za zbyt optymistyczne. Widoczne na mapie wektorowej krótkie odcinki (oznaczone A), przecinające cechy geometryczne o większej długości, powstały wskutek błędnej estymacji kierunku prostych wspierających na podstawie niewielkiej liczby punktów. Skutkiem niedoskonałości procesu wyodrębniania cech jest też zniekształcenie obiektu, oznaczone literą D (rzeczywisty kształt widoczny jest na rys. 4.4B). Drugi eksperyment wykonano w korytarzu budynku Wydziału Elektrycznego. Robot poruszał się po ścieżce o długości około 45 m, zaplanowanej tak, by powrócił w pobliże punktu startu. Na rysunkach 4.6A 4.6E przedstawiono widoki mapy wektorowej w wybranych fazach tego eksperymentu. Dla ilustracji koncepcji reprezentacji SPmap dokonano także wizualizacji lokalnych układów współrzędnych poszczególnych cech geometrycznych 1. Na rysunku 4.6F zilustrowano natomiast zdolność robota do samolokalizacji za pomocą metody EKF-SLAM. Pokazano na nim elipsy niepewności położenia odczytywanego z odometrii (linia ciągła) i elipsy niepewności położenia estymowanego za pomocą EKF-SLAM (linia przerywana). Przedstawione mapy dowodzą, że opracowana implementacja EKF-SLAM pozwala robotowi Labmate tworzyć model nieznanego otoczenia także poza laboratorium, gdy środowisko nie jest zaadaptowane do możliwości percepcyjnych skanera laserowego. Jednak podobnie jak w eksperymencie prowadzonym w laboratorium, mapa wektorowa utworzona metodą EKF-SLAM (rys. 4.6F) składa się z wielu odcinków, w niektórych obszarach nakładających się na siebie lub się przecinających. Jest to dobrze widoczne na rys. 4.7, na którym cechy geometryczne przedstawiono jako płaszczyzny symbolizujące ściany w rzucie perspektywicznym. Korytarz o gładkich ścianach jest środowiskiem, które może być dobrze aproksymowane za pomocą przyjętych cech geometrycznych odcinków, a skaner LMS 200 zapewnia precyzyjne pomiary odległości. Widoczne niedokładności na mapie są więc głównie skutkiem nieprawidłowego kojarzenia cech geometrycznych i obserwacji będących jedynie częściowymi reprezentacjami struktur geometrycznych występujących w środowisku. Poprzednie eksperymenty prowadzono w środowiskach, których geometrię bardzo dobrze można było opisać za pomocą odcinków. Zgodnie z koncepcją tych eksperymentów, pozwo- 1 Pozostałe mapy w niniejszym rozdziale zaprezentowano bez układów współrzędnych cech, gdyż uznano, że pogarszają one czytelność rysunków. 172

A B C D E F Rysunek 4.6. Proces tworzenia mapy podczas eksperymentu w korytarzu budynku WE 173

Rysunek 4.7. Trójwymiarowa wizualizacja mapy uzyskanej podczas eksperymentu w korytarzu liło to uzyskać mapy wektorowe stosunkowo dokładnie odzwierciedlające kształty otoczenia, co z kolei umożliwia łatwiejszą interpretację wyników. A B Rysunek 4.8. Przykładowe mapy uzyskane w środowisku niezmodyfikowanym (opis w tekście) Niektóre metody samolokalizacji, oparte na zasadzie dopasowywania mapy lokalnej do globalnej i na odcinkach wyodrębnionych z danych skanera laserowego, wymagają istnienia w otoczeniu płaskich, pionowych powierzchni (ścian) o określonych minimalnych wymiarach [240]. W metodzie EKF-SLAM nie są natomiast wymagane cechy geometryczne o określonych wymiarach. Stochastyczna reprezentacja otoczenia SPmap odwołuje się wyłącznie do pozycji cech, a długości odcinków są używane jedynie podczas dopasowywania obserwacji do mapy. Możliwa jest więc samolokalizacja i tworzenie mapy w środowisku słabo ustrukturalizowanym, w którym znajduje się wiele obiektów trudnych dla algorytmów ekstrakcji cech. W takim środowisku liczba cech wyodrębnionych ze skanu jest mniejsza. Powstała mapa stanowi wystarczającą podstawę samolokalizacji, lecz nie reprezentuje dobrze geometrii otoczenia. Przykładem środowiska o takim charakterze jest np. pomieszczenie la- 174

boratorium, którego nie dostosowano do możliwości percepcyjnych robota. Wyniki działania implementacji EKF-SLAM w tym środowisku przedstawiono na rys. 4.8. Na rysunku 4.8A pokazano mapę wektorową uzyskaną po pokonaniu przez robota ścieżki o kształcie okręgu, a na rys. 4.8B mapę powstałą podczas przejazdu po ścieżce o kształcie prostokąta. Eksperymenty wykonano w pewnym odstępie czasu, konfiguracja obiektów na obu mapach jest więc różna. Na obu rysunkach zaznaczono elipsy niepewności położenia robota, otrzymane zarówno na podstawie odometrii, jak i estymowane metodą EKF-SLAM. Wyniki ilościowe Uzupełnieniem przedstawionej w poprzednim podrozdziale analizy jakościowej map otoczenia uzyskanych metodą EKF-SLAM są wyniki ilościowe dotyczące niepewności położenia robota podczas przejazdu po zadanej ścieżce. Za syntetyczną miarę niepewności przyjęto pole elipsy wyznaczonej na podstawie macierzy (4.17) za pomocą wzorów (2.22). pole elipsy niepewnoœci po³o enia [mm 2 ] stopieñ dopasowania obserwacji [%] 7 x 104 6 5 4 3 2 1 A 0 0 0 100 200 300 400 500 600 700 800 900 1000 0 100 200 300 400 500 600 700 800 900 1000 przebyta odleg³oœæ [cm] przebyta odleg³oœæ [cm] 100 0.2 90 80 70 60 50 40 30 20 10 EKF-SLAM ODOMETRIA 0 0 100 200 300 400 500 600 700 800 900 1000 przebyta odleg³oœæ [cm] C o odchylenie standardowe orientacji [ ] czas wykonania etapu estymacji [s] 12 10 8 6 4 2 0.18 0.16 0.14 0.12 0.1 0.08 0.06 0.04 0.02 B D EKF-SLAM ODOMETRIA 0 0 100 200 300 400 500 600 700 800 900 1000 przebyta odleg³oœæ [cm] Rysunek 4.9. Wyniki uzyskane podczas eksperymentu w laboratorium (opis w tekście) Możliwość korekcji pozycji robota i estymowana niepewność tej pozycji w danej chwili zależy od liczby obserwowanych cech geometrycznych oraz skuteczności dopasowania obserwacji i cech. Syntetycznym wskaźnikiem jakości procesu kojarzenia obserwacji i cech jest to, jaka część spośród wszystkich obserwacji dokonanych w danym kroku została dopasowana do mapy. Sytuacja, w której jedynie niewielka część spośród wielu dokonanych obserwacji została dopasowana do mapy, może świadczyć o nieprawidłowym działaniu EKF. 175

Bardzo istotna z praktycznego punktu widzenia jest także możliwość działania EKF-SLAM w czasie rzeczywistym, czyli przetwarzania danych ze skanera laserowego z szybkością nie mniejszą niż szybkość ich akwizycji. Ponieważ o złożoności obliczeniowej EKF-SLAM decyduje złożoność etapu estymacji, podczas eksperymentów mierzono czas trwania tego etapu. Na rysunku 4.9 zaprezentowano wyniki ilościowe uzyskane podczas eksperymentu wykonanego w laboratorium. Wykresy niepewności położenia (rys. 4.9A) i orientacji (rys. 4.9B) w funkcji przebytej odległości pozwalają na porównanie wyników pozycjonowania za pomocą EKF-SLAM i odometrii. Niepewność położenia wyznaczanego za pomocą odometrii rośnie w sposób nieograniczony, natomiast niepewność położenia estymowana metodą EKF-SLAM jest mała i nie rośnie wraz z przebytą drogą. Wzrost niepewności pozycji estymowanej metodą SLAM następuje w punktach ścieżki, w których szybko rośnie także niepewność wyznaczona na podstawie informacji z odometrii. Punkty te odpowiadają obrotowi robota w miejscu. Powstające podczas takiego obrotu duże błędy orientacji określanej za pomocą odometrii (por. p. 2.2) są przyczyną niedokładnej predykcji niepewności stanu robota (4.48), a dalej prowadzą do niepowodzenia statystycznej metody dopasowywania cech (4.60). W kolejnych punktach ścieżki liczba dopasowanych cech (rys. 4.9C) jest wystarczająca, by skorygować estymatę pozycji robota, jednak do mapy globalnej dodawanych jest wiele niedopasowanych cech widocznych na rys. 4.6. Konsekwencją tego jest mapa o jakości gorszej od oczekiwanej. pole elipsy niepewnoœci po³o enia [mm 2 ] stopieñ dopasowania obserwacji [%] 9 x 105 8 7 6 5 4 3 2 1 A 0 0 500 1000 1500 2000 2500 3000 3500 4000 4500 przebyta odleg³oœæ [cm] 100 90 80 70 60 50 40 30 20 10 C EKF-SLAM ODOMETRIA 0 0 500 1000 1500 2000 2500 3000 3500 4000 4500 przebyta odleg³oœæ [cm] odchylenie standardowe orientacji [ o ] czas wykonania etapu estymacji [s] 12 10 8 6 4 2 0 0 500 1000 1500 2000 2500 3000 3500 4000 4500 przebyta odleg³oœæ [cm] 0.16 0.14 0.12 0.1 0.08 0.06 0.04 0.02 B D EKF-SLAM ODOMETRIA 0 0 500 1000 1500 2000 2500 3000 3500 4000 4500 przebyta odleg³oœæ [cm] Rysunek 4.10. Wyniki uzyskane podczas eksperymentu w korytarzu (opis w tekście) 176

Zmierzony podczas eksperymentu czas integracji nowych obserwacji z mapą globalną (rys. 4.9D) rośnie wraz z przebytą drogą, co wynika ze zwiększania się całkowitej liczby cech geometrycznych na mapie globalnej. Wzrost liczby tych cech na mapie globalnej jest z jednej strony oczywistą konsekwencją eksploracji nieznanego otoczenia i poznawania przez robota kolejnych jego fragmentów, ale z drugiej wynika z dodawania do mapy w każdym kroku wszystkich niedopasowanych cech. Nieskuteczne kojarzenie cech jest więc także czynnikiem zwiększającym koszt obliczeń w kolejnych krokach EKF-SLAM. Na rysunku 4.10 przedstawiono wyniki ilościowe uzyskane podczas eksperymentu prowadzonego w korytarzu pierwszego piętra budynku Wydziału Elektrycznego Politechniki Poznańskiej. Zmniejszenie niepewności estymaty położenia i orientacji w porównaniu z samolokalizacją za pomocą odometrii przedstawiono na rys. 4.10A i 4.10B. Niepewność położenia estymowana metodą EKF-SLAM jest bardzo mała, co z uwagi na jakość mapy wektorowej (rys. 4.5) należy uznać za oszacowanie zbyt optymistyczne. Ponieważ jedynie integracja z wektorem stanu systemu nowych obserwacji cech znajdujących się już na mapie pozwala zredukować niepewność pozycji robota (4.63), to niepewność ta zależy bezpośrednio od liczby obserwacji dopasowanych w danym kroku. Liczbę dopasowanych cech w stosunku do liczby wszystkich cech zaobserwowanych w danym kroku przedstawiono na rys. 4.10C. Porównanie tego wykresu z rys. 4.10A i 4.10B pozwala stwierdzić, że punkty, w których niepewność położenia i orientacji rośnie, odpowiadają punktom, w których dopasowano stosunkowo niewiele cech. Czas integracji nowych obserwacji z mapą globalną (rys. 4.10D) rośnie wraz z przebytą drogą, podobnie jak w poprzednio analizowanym eksperymencie. Mimo że w tym przypadku robot pokonał dłuższą trasę i utworzył mapę obejmującą znacznie większy obszar, to czas trwania etapu estymacji przy końcu eksperymentu był zbliżony do uzyskanego w poprzednim eksperymencie. Wynika to z tego, że w korytarzu jest mniej cech postrzeganych przez skaner laserowy niż w laboratorium i w rezultacie obie mapy, pomimo różnicy rozmiarów, są zbudowane z podobnej liczby cech. 4.3.5. Wnioski Podstawowe problemy praktycznego wykorzystania metody EKF-SLAM wiążą się z: analizą danych uzyskanych z sensorów zewnętrznych i niezawodną ekstrakcją cech geometrycznych; reprezentacją cech geometrycznych oraz ich niepewności; skutecznym kojarzeniem obserwacji i cech znajdujących się na mapie; złożonością obliczeniową algorytmu estymacji stanu. Przedstawione w poprzednim podrozdziale oraz w podrozdziale 3.3 wyniki eksperymentów wskazują, że proponowane w niniejszej pracy procedury przetwarzania danych pochodzących ze skanera laserowego, a w szczególności nowa metoda grupowania pomiarów i odpornej estymacji prostej wspierającej, są wystarczająco skuteczne. Badania prowadzono jednak w środowisku statycznym, bez uwzględnienia problemów stwarzanych przez obiekty dynamiczne. Z występowaniem zakłóceń pochodzących od tego rodzaju obiektów należy się liczyć w rzeczywistych aplikacjach robotów autonomicznych, np. w miejscach publicznych, gdzie obiektami dynamicznymi są ludzie [141, 199]. 177

Problem matematycznego opisu cech geometrycznych, spełniającego wymagania metody EKF-SLAM, rozwiązano, stosując reprezentację SPmodel. Implementacja tej reprezentacji w postaci obiektowej (biblioteki klas) [323] stanowi szkielet kolejnych wersji oprogramowania realizującego algorytm EKF-SLAM, opracowywanych w IAiII. Analizując wyniki działania przedstawionej implementacji algorytmu EKF-SLAM, można dojść do wniosku, że zaobserwowane niedoskonałości, a szczególnie gorsza od oczekiwanej jakość i topologiczna spójność tworzonych map wektorowych, są spowodowane przede wszystkim nieskutecznym dopasowywaniem obserwacji do istniejących już cech geometrycznych. W analizowanych przykładach liczba poprawnych dopasowań cech jest wystarczająca do samolokalizacji, jednak na mapie globalnej pozostaje wiele niedopasowanych, zbędnych cech, w znacznym stopniu pogarszających jej jakość oraz użyteczność z punktu widzenia całego systemu nawigacji robota (np. jako podstawy planowania ścieżki). Jest to istotna niedoskonałość, gdyż efektywna procedura kojarzenia obserwacji i cech nie tylko umożliwia budowę lepszej mapy wektorowej, ale jest także warunkiem koniecznym rozwiązania problemu zamykania pętli (ang. loop-closing problem), czyli utworzenia spójnej topologicznie mapy w sytuacji, gdy robot powraca do obszaru, który już wcześniej eksplorował [199, 307]. W przedstawionej w niniejszej pracy implementacji algorytmu EKF-SLAM, podobnie jak w większości rozwiązań opisywanych w literaturze [23, 27, 71, 122, 141, 182, 219], dopasowanie dokonywane jest na zasadzie najbliższego sąsiedztwa. W świetle wyników eksperymentów oraz przedstawionej w rozdziale 2 analizy niepewności pomiarów pochodzących z używanych sensorów za podstawową przyczynę niewystarczającej skuteczności strategii najbliższego sąsiedztwa uznać można zbyt dużą rzeczywistą niepewność pozycji robota oraz zbyt optymistyczne (zaniżone) oszacowanie tej niepewności dokonywane za pomocą modelu odometrii. Model ten ma decydujący wpływ na dokładność estymaty stanu otrzymywanej na etapie predykcji. Z uwagi na wyniki kalibracji odometrii robota TRC Labmate, podane w podrozdziale 2.2, jest także prawdopodobne, że negatywny wpływ na działanie EKF ma składowa systematyczna niepewności odometrii, której nie udało się całkowicie wyeliminować. W szczególności wskazuje na to wyraźne pogorszenie wyników dopasowywania obserwacji w sytuacji, gdy robot obraca się w miejscu. 4.4. Zastosowanie sensorów logicznych w systemie SLAM 4.4.1. Koncepcja modyfikacji systemu EKF-SLAM Wyniki doświadczeń z przedstawioną w poprzednim rozdziale implementacją systemu SLAM wykazały istnienie ograniczeń związanych z przyjętym sposobem ekstrakcji cech geometrycznych oraz problemów wynikających z niespełniania przez rzeczywisty system istotnych założeń leżących u podstaw metody EKF-SLAM. W przedstawionej implementacji, podobnie jak w większości systemów SLAM działających w pomieszczeniach zamkniętych, wykorzystano skaner laserowy. Użyty skaner LMS 200 pozwala dokonywać precyzyjnych pomiarów odległości, jednak eksperymenty opisane w podrozdziale 2.4.3 wykazały, że sensor ten nie jest wolny od błędów systematycznych oraz problemów związanych z niepewnością co do przedmiotu pomiaru, której źródłem są efekt śladu optycznego oraz szczególne rodzaje obserwowanych powierzchni (przeźroczyste, 178

zwierciadlane). Istnienie niepewności co do przedmiotu pomiaru w danych będących podstawą wyodrębniania cech geometrycznych utrudnia działanie EKF-SLAM, gdyż niepewność pozycji cech, wyrażona w postaci macierzy kowariancji, nie uwzględnia tego rodzaju niepewności (wynika to z założenia o reprezentacji niepewności za pomocą rozkładów normalnych), a to z kolei utrudnia podjęcie decyzji, które z obserwowanych cech geometrycznych pasują do istniejącej mapy globalnej. Gdy robot wyposażony w skaner laserowy operuje w środowisku, w którym są obiekty dynamiczne (ludzie, pojazdy, inne roboty), istotnie wzrasta obciążenie odtwarzanych cech geometrycznych niepewnością co do przedmiotu pomiaru. Najprostszym rozwiązaniem problemu niepewności co do przedmiotu pomiaru w przypadku mapy wektorowej jest wykorzystywanie jedynie tych pomiarów, które dają najbardziej stabilne cechy geometryczne, np. znacznej długości odcinki odpowiadające fragmentom ścian, oraz ignorowanie pozostałych pomiarów. Takie podejście może poprawić niezawodność samolokalizacji opartej na EKF [141], jednak powstająca wówczas mapa wektorowa traci walory uniwersalnej reprezentacji otoczenia, przydatnej także do planowania ruchu. Jeżeli system SLAM ma utworzyć mapę wektorową odzwierciedlającą geometrię eksplorowanego środowiska, to należy znaleźć inne rozwiązanie. Drugim rodzajem sensorów wykorzystywanych w zaimplementowanym systemie EKF-SLAM są enkodery na kołach robota. Na wykorzystaniu tych sensorów, monitorujących ruch robota, jest oparty etap predykcji filtru Kalmana w metodzie EKF-SLAM. Przyczyną problemów związanych z wykorzystaniem odometrii w EKF-SLAM jest niemożność uwzględnienia wszystkich źródeł błędów w modelu niepewności samolokalizacji zliczeniowej, w której wykorzystuje się dane z enkoderów. W rezultacie, nie wszystkie aspekty niepewności związanej z odometrią są prawidłowo modelowane za pomocą macierzy kowariancji (4.23). Szczególnie istotne są błędy orientacji. Jeżeli ich rzeczywiste wartości nie będą uwzględnione w modelu niepewności, to spowoduje to duże błędy linearyzacji w jakobianach (4.57 4.59), a następnie dywergencję filtru Kalmana [74]. Istotna jest także kalibracja systematycznej składowej błędów odometrii, ponieważ w algorytmie EKF założono brak błędów systematycznych. Jeżeli takie błędy występują, to prowadzą do nieprawidłowego działania filtru Kalmana [30]. W przedstawionym w podrozdziale 2.2 modelu niepewności odometrii robota TRC Labmate uwzględnia się jedynie niepewność powodowaną błędami przypadkowymi. Model ten nie pozwala uwzględnić takich źródeł błędów, jak kolizje lub nierówności podłoża. Błędy powodowane przez tego rodzaju nieprzewidywalne czynniki zewnętrzne nie mogą być uwzględnione w żadnym statystycznym modelu niepewności [52]. Przyjęty model niepewności jest słuszny jedynie w wąskim zakresie warunków ruchu robota, a wykrycie i korekcja błędów wynikających z niespełnienia założeń leżących u podstaw koncepcji samolokalizacji zliczeniowej są możliwe jedynie dzięki odwołaniu się do innych sensorów. Wyniki eksperymentów pokazały, że złożoność obliczeniowa algorytmu EKF-SLAM uniemożliwia aktualizację mapy wektorowej z częstotliwością odpowiadającą częstotliwości akwizycji danych ze skanera LMS 200. Ponieważ złożoność ta jest proporcjonalna do kwadratu liczby cech na mapie wektorowej, to pożądana jest reprezentacja otoczenia za pomocą jak najmniejszej liczby cech geometrycznych i eliminowanie cech redundantnych, reprezentujących tę samą strukturę geometryczną środowiska. Jednym ze sposobów spowolnienia procesu rozrostu mapy (wektora stanu EKF-SLAM) jest zastosowanie bardziej restrykcyjnej metody wyodrębniania cech geometrycznych, która pozwoli zredukować liczbę cech generowanych w poszczególnych krokach i zmniejszy prawdopodobieństwo uzyskiwania cech geometrycznych na podstawie danych obarczonych 179

niepewnością co do przedmiotu pomiaru. Jest to korzystne także w kontekście kojarzenia obserwacji i cech, ponieważ cechy powstałe na podstawie mało wiarygodnych danych także charakteryzują się niepewnością co do przedmiotu pomiaru, a dołączenie takiej cechy do wektora stanu EKF-SLAM może być w kolejnych krokach przyczyną nieprawidłowych dopasowań. Podobne ograniczenia i problemy podczas eksperymentalnej weryfikacji opartych na algorytmie EKF-SLAM systemów jednoczesnej samolokalizacji i tworzenia mapy napotkali autorzy wielu publikacji [23, 74, 141, 199]. Za pomocą typowych implementacji EKF-SLAM uzyskuje się mapy złożone z cech punktowych lub krótkich odcinków, które nie pozwalają reprezentować bardziej złożonych struktur otoczenia, mimo że implementacje te umożliwiają skuteczną samolokalizację robota. Problemy związane z ekstrakcją cech geometrycznych w środowiskach, w których występują obiekty dynamiczne, rozwiązywane są niekiedy przez zastosowanie specyficznych konfiguracji sensorów (np. skaner laserowy na ruchomej głowicy [141] lub skaner 3D [327]), lecz w większości opisywanych w literaturze implementacji EKF-SLAM ignoruje się ten problem, odwołując się do założenia o statycznym otoczeniu i wykorzystując fakt, że metody ekstrakcji cech geometrycznych działają jak swoiste filtry danych wejściowych i odrzucają np. małoliczne skupiska pomiarów [23]. Nieskorygowane błędy systematyczne oraz nieprzewidywalne błędy katastrofalne odometrii opartej na odczytach z enkoderów są uważane za jedną z częstszych przyczyn problemów praktycznego wykorzystania EKF-SLAM [199]. Część błędów odometrii można skorygować, gdy dysponuje się danymi z innych sensorów wewnętrznych. W szczególności zastosowanie żyroskopu [32, 52] lub kompasu elektronicznego pozwala znacznie zmniejszyć błąd orientacji, co jak wykazano za pomocą symulacji [143] skutecznie zapobiega dywergencji EKF w systemie SLAM. Inne podejście do wykorzystania odometrii w EKF-SLAM przedstawiono w pracy [199], gdzie zaproponowano odsprzężenie estymacji pozycji cech i predykcji pozycji robota przez użycie mapy zawierającej jedynie cechy niezmiennicze względem przesunięcia i obrotu robota. W praktyce w metodzie tej wykorzystuje się cechy punktowe lub narożniki, co powoduje, że tworzona w ten sposób mapa wektorowa nie nadaje się do planowania ruchu robota. W świetle powyższej analizy przyczyn problemów podczas praktycznego zastosowania algorytmu EKF-SLAM zdecydowano się na modyfikację systemu jednoczesnej samolokalizacji i tworzenia mapy przedstawionego w poprzednim podrozdziale. Podstawowym celem była taka modyfikacja oprogramowania, aby za jego pomocą uzyskiwać mapy wektorowe lepszej jakości, tzn. mapy o mniejszej liczbie nieprawidłowych i redundantnych cech oraz bez istotnych zniekształceń geometrycznych. Jednocześnie zmodyfikowany system SLAM powinien móc działać w obecności pojedynczych obiektów dynamicznych w polu widzenia skanera przy założeniu, że tworzona mapa będzie reprezentować wyłącznie część statyczną otoczenia. Uznano, że dla polepszenia własciwości istniejącego systemu SLAM należy rozwiązać dwa zagadnienia: eliminacji tych danych ze skanera, które są obciążone niepewnością co do przedmiotu pomiaru; niezależnej od odometrii estymacji przemieszczenia robota pomiędzy kolejnymi punktami, w których dokonywane są obserwacje. Proponowane rozwiązanie opiera się na maksymalnym wykorzystaniu informacji pochodzących ze skanera laserowego, który charakteryzuje się największą spośród sensorów robota 180

TRC Labmate, precyzją pomiarów, najmniejszą podatnością na błędy prowadzące do niepewności co do przedmiotu pomiaru oraz wiarygodnym modelem niepewności [287]. Do eliminacji danych obciążonych niepewnością co do przedmiotu pomiaru zastosowano mapę rastrową, która akumuluje dane uzyskane z wielu kolejnych skanów, dając tym samym podstawę do wnioskowania o dynamice danego fragmentu otoczenia. Ekstrakcja cech geometrycznych na potrzeby EKF-SLAM ze wsparciem reprezentacji rastrowej wykonywana jest metodami przedstawionymi w podrozdziale 3.4.4. Ponieważ skaner LMS 200 umożliwia wykonywanie skanów otoczenia z częstotliwością większą niż częstotliwość akwizycji danych na potrzeby EKF-SLAM, zaproponowano metodę dopasowywania skanów jako sposób estymacji przemieszczenia robota pomiędzy kolejnymi punktami, w których dokonywana jest estymacja pozycji i mapy za pomocą EKF. Metody te stanowią rozszerzenie poprzednio zaprezentowanego systemu EKF-SLAM, gdyż pozwalają poprawić jakość estymaty przemieszczenia robota na etapie predykcji EKF oraz wspierają ekstrakcję cech geometrycznych odporną na błędy związane z obiektami dynamicznymi i niepewnością co do przedmiotu pomiaru. Ponieważ obiema metodami dokonuje się ekstrakcji odmiennego, specyficznego rodzaju informacji z danych pobieranych ze skanera laserowego, można traktować je jako sensory logiczne (ang. logical sensor) [128]. Z tego powodu zmodyfikowaną wersję systemu EKF-SLAM nazwano LS/EKF-SLAM. W rozdziale 5 przedstawiono propozycję architektury programowej opartej na koncepcji sensorów logicznych i systemu tablicowego, która pozwala zintegrować podstawowe moduły funkcjonalne autonomicznego robota mobilnego. 4.4.2. Ekstrakcja cech geometrycznych ze wsparciem reprezentacji rastrowej Efektywne działanie systemu SLAM opartego na wykorzystaniu mapy wektorowej, na której cechami geometrycznymi są odcinki, jest uwarunkowane prawidłowym wyodrębnieniem tych odcinków z danych otrzymanych ze skanera laserowego. W szczególności pożądana jest eliminacja na wczesnym etapie przetwarzania cech geometrycznych związanych z obiektami dynamicznymi oraz wykorzystanie procedur odpornych na błędy nadmierne sporadycznie pojawiające się w danych, np. na skutek efektu śladu optycznego. W podrozdziale 3.4.4 przedstawiono metodę ekstrakcji cech, która łączy zalety metody opartej na algorytmie IEPF (użytej w implementacji EKF-SLAM przedstawionej w p. 4.3) oraz mapy rastrowej wykorzystanej jako narzędzie oceny wiarygodności pomiarów. Metodę tę wykorzystano do wyodrębniania cech-odcinków na potrzeby zmodyfikowanego systemu EKF-SLAM. Procedura tworzenia mapy rastrowej FSG oraz wyznaczania geometrycznych parametrów odcinków prowadzona jest w sposób opisany w podrozdziale 3.4.4. Pomiary grupowane są z użyciem algorytmu 3.2, a pomiary zaakceptowane na podstawie przynależności do komórek mapy rastrowej o odpowiednio dużej wartość µ S są poddawane segmentacji za pomocą algorytmu IEPF na grupy w przybliżeniu współliniowych punktów odpowiadających odcinkom. Dla każdego z odcinków oblicza się parametry równania linii wspierającej. Analogicznie jak we wcześniejszej wersji systemu EKF-SLAM, składniki wektora pozycji cechy (4.44) wyznacza się na podstawie parametrów prostej wspierającej oraz współrzędnych punktów końcowych odcinka, które określa się przez rzutowanie skrajnych punktów otrzymanych z pomiarów. Macierz kowariancji C E oblicza się z użyciem EIF, jednak pod uwagę bierze się tylko macierze kowariancji punktów zaakceptowanych na podstawie analizy mapy rastrowej. Reprezentację cechy uzupełnia się o długość odcinka l E. 181

Cechy geometryczne wyodrębnia się ze wsparciem reprezentacji rastrowej w punktach ścieżki odległych od siebie o 0,5 m, co przy prędkości robota wynoszącej 0,2 m s i częstotliwości skanowania 4 Hz pozwala na użycie co dziesiątego skanu. Pozostałe pomiary służą jedynie do aktualizacji mapy FSG. 4.4.3. Estymacja przemieszczenia robota Dopasowywanie skanów jako metoda estymacji przemieszczenia robota Przesunięcie i obrót odpowiadające przemieszczeniu się robota pomiędzy dwoma pozycjami mogą być określone za pomocą dopasowywania skanów (ang. scan matching), tzn. przez znalezienie takiego przesunięcia i obrotu robota, które maksymalizują nakładanie się skanów w kolejnych pozycjach. Dopasowywanie skanów można uznać za pewną odmianę metody samolokalizacji przez dopasowywanie map. Technika ta przybiera różne formy w zależności od przyjętej reprezentacji pomiarów (skanu) i mapy otoczenia. W literaturze są opisywane algorytmy dopasowujące nieprzetworzone skany (dopasowanie punkt punkt) [123, 187, 234], dopasowujące skany do mapy globalnej złożonej z cech geometrycznych (dopasowanie punkt odcinek) [77] oraz wykorzystujące pośrednią reprezentację skanu, np. w postaci histogramu [94, 321]. Metoda dopasowywania skanów stosowana jest przede wszystkim w systemach samolokalizacji opartych na skanerach laserowych 2D, ale podobne algorytmy są używane w rekonstrukcji obrazów trójwymiarowych [45], a techniki tego rodzaju mogą być dostosowane do danych uzyskanych z innych sensorów, np. wizyjnych [299]. Dopasowywanie skanów jest wykorzystywane w wielu systemach samolokalizacji robotów ze znaną mapą [77, 123], może też być użyte jako podstawa metody SLAM. W metodzie tej tworzona mapa jest zbiorem skanów referencyjnych, do których są dopasowywane kolejne skany wykonywane przez robota [188, 321]. Do wyznaczenia estymaty przemieszczenia robota pomiędzy dwoma kolejnymi punktami, w których wykonywane są obserwacje używane w EKF-SLAM, wystarczy dopasowanie dwóch skanów zarejestrowanych w tych punktach. Skuteczność dopasowywania skanów jako metody korekcji błędów odometrii robota TRC Labmate sprawdzono eksperymentalnie, implementując algorytm histogramów kątowych [321] oraz algorytm histogramów skumulowanej długości segmentów [94]. Otrzymane wyniki [261] potwierdziły w szczególności możliwość uzyskiwania na podstawie informacji ze skanera estymaty orientacji znacznie dokładniejszej niż estymata otrzymywana za pomocą odometrii robota TRC Labmate. W literaturze opisano wiele algorytmów dopasowywania skanów. Niestety, większość z nich nie zawiera metody wyznaczania macierzy kowariancji przemieszczenia C, która jest konieczna do wykorzystania alternatywnej odometrii na etapie predykcji algorytmu EKF-SLAM. Wadą tą obarczone są między innymi oba badane w pracy [261] algorytmy oparte na korelacji histogramów [94, 321]. W przypadku wykorzystywanego w wielu systemach samolokalizacji algorytmu IDC (ang. Iterative Dual Correspondence) [187] macierz kowariancji przemieszczenia wyznaczana jest metodami numerycznymi [40], bez uwzględnienia modelu niepewności używanego sensora. Jedną z metod estymacji przemieszczenia robota na podstawie dopasowywania skanów, która pozwala wyznaczyć macierz kowariancji C z uwzględnieniem modelu niepewności pomiarów, jest algorytm WLSM (ang. Weighted Laser Scan Matching) [234]. Algorytm ten, oparty na dopasowywaniu punktów (nieprzetworzonych skanów), w jawny sposób modeluje źródła błędów zarówno w pomiarach odległości dokonywanych skanerem laserowym, jak 182

i w samym procesie dopasowywania punktów należących do obu skanów. Na podstawie znajomości źródeł błędów możliwe jest realistyczne oszacowanie niepewności przemieszczenia robota. W przyjętym w pracy [234] modelu niepewności pomiaru wyróżniono błędy systematyczne i przypadkowe, zależne od mierzonej odległości oraz od kąta padania wiązki na obserwowaną powierzchnię, co odpowiada strukturze przedstawionego w podrozdziale 2.4.3 modelu niepewności pomiarów skanerem LMS 200. Procedura dopasowywania skanów składa się z dwóch podstawowych etapów: kojarzenia punktów należących do skanów wykonanych w dwóch różnych pozycjach robota oraz estymacji przemieszczenia robota pomiędzy tymi pozycjami. Kojarzenie punktów Wstępnym etapem przetwarzania każdego skanu jest korekcja błędów systematycznych pomiaru odległości metodą podaną w podrozdziale 2.4.3 oraz grupowanie metodą opisaną algorytmem 3.2. Podczas etapu grupowania eliminowane są pomiary izolowane i grupy, w których liczba pomiarów jest mniejsza niż założone minimum. Pozwala to usunąć pomiary powstałe w wyniku efektu śladu optycznego oraz część artefaktów spowodowanych dynamiką otoczenia. Przemieszczenie robota wyznacza się na podstawie współrzędnych n ij par odpowiadających sobie (dopasowanych) punktów u i k, uj k (k = 1,...n ij) skanów wykonanych w pozycjach i oraz j. W rzeczywistym procesie skanowania otoczenia punkty te nie zawsze odpowiadają dokładnie tym samym miejscom na powierzchni obserwowanych obiektów (rys. 4.12). Współrzędne kartezjańskie punktów otrzymanych w i-tej pozycji robota oznaczane są jako u i k (k = 1,...n p). Punkty te są opisane w układzie współrzędnych robota za pomocą wzorów: [ ] u i x i k = k y i k = r i k [ cos ϕ i k sinϕ i k ], (4.67) gdzie rk i jest zmierzoną odległością, a ϕi k kątem pomiędzy k-tą wiązką skanera a osią 0X układu współrzędnych robota. W opisywanej implementacji wykorzystano także mapę rastrową FSG, która pozwala usunąć pozostałe punkty związane z obiektami dynamicznymi. W procesie dopasowywania uczestniczą wyłącznie te punkty i-tego skanu, które leżą w komórkach mapy rastrowej o wystarczająco dużej wartości µ S. Duża wartość przynależności do zbioru support (por. p. 3.4.2) świadczy o tym, że w danej komórce mapy odnotowano już pomiary w poprzednich krokach. Na etapie kojarzenia punktów korzysta się z prostej zasady dopasowania tych punktów należących do obu skanów, których odległość euklidesowa jest najmniejsza (ang. closest-point rule) [45]. W literaturze opisano metody kojarzenia punktów, które są potencjalnie skuteczniejsze niż zasada najbliższego punktu, szczególnie przy większych początkowych błędach przemieszczenia robota [187, 188]. Są to jednak metody kosztowniejsze obliczeniowo. Ponieważ przedstawiona tutaj metoda dopasowywania skanów służy do estymacji przemieszczenia między relatywnie mało odległymi pozycjami robota, zdecydowano się zaimplementować metodę mniej ogólną, lecz także mniej złożoną. Dla każdego punktu i-tego skanu znajduje się odpowiadający mu punkt skanu j, który spełnia kryterium maksymalnej odległości: u i k uj k < d (rys. 4.11). Jeżeli dla danego punktu skanu dokonanego w pozycji i żaden punkt skanu dokonanego w pozycji j nie spełnia powyższego warunku, to dany punkt skanu i oznaczany jest jako nieskojarzony i nie uczestniczy w dalszym przetwarzaniu. Odległość d dobierana jest na podstawie spodziewanego 183

Rysunek 4.11. Przykłady dopasowania punktów należących do dwóch kolejnych skanów maksymalnego błędu określenia położenia robota na podstawie odometrii. Spośród punktów spełniających warunek maksymalnej odległości wybierane są pary, których odległość euklidesowa jest najmniejsza. Estymacja przemieszczenia i jego macierzy kowariancji Na etapie estymacji przemieszczenia robota wykorzystywany jest iteracyjny algorytm WLSM. Szczegółowy opis tej metody wraz z wyprowadzeniami wzorów i dowodem optymalności wyznaczanych estymat zainteresowany czytelnik znajdzie w pracy [234]. Poniżej przedstawiono podstawowe wzory i procedurę obliczeniową umożliwiającą wyznaczenie estymaty względnego przemieszczenia ˆx ij R pomiędzy pozycjami i oraz j, złożonego z translacji ˆt ij i rotacji ˆθ ij. Wektor translacji i macierz rotacji zdefiniowane są następująco: t ij = [ xij y ij ], R ij = [ cos θij sin θ ij sin θ ij cos θ ij ]. (4.68) Biorąc pod uwagę geometrię procesu skanowania powierzchni obiektów (rys. 4.12), błąd dopasowania pary punktów po przemieszczeniu opisanym t ij i R ij można wyrazić jako ε ij k = ui k R ij u j k t ij. (4.69) Zadaniem algorytmu WLSM jest wyznaczenie takiej estymaty przemieszczenia ˆt ij, ˆθ ij, która minimalizuje błąd wyrażony równaniem (4.69) dla wszystkich par skojarzonych punktów. Gdy dopasowanie punktów skanów jest ustalone, estymaty translacji i rotacji wyznacza się za pomocą iteracyjnego algorytmu, dla którego początkową estymatą przemieszczenia kątowego ˆθ 0 jest wartość odczytana z odometrii. Gdy znane jest przemieszczenie kątowe, to przemieszczenie liniowe ˆt można obliczyć za pomocą następującej formuły: ( nij ˆt ij = P tt k=1 (P ij k ) 1 ( u i k ˆR ij u j k)), (4.70) 184

u k+1 i i i j u u k u k k+1 k i u k-1 i u k-1 j j k k-1 xi yi k i y j k j k r k-1 r k x j Rysunek 4.12. Geometria procesu skanowania (na podstawie [234]) Rysunek 4.13. Sposób określania kąta padania wiązki gdzie ˆR ij = ˆR ij ( ˆθ ij ) i jest macierzą rotacji wyznaczoną na podstawie dostępnej estymaty przemieszczenia kątowego, a macierz kowariancji translacji P tt dana jest wzorem: P tt = ( nij (P ij k=1 k ) 1) 1. (4.71) Estymata translacji uzyskana ze wzoru (4.70) wykorzystywana jest do aktualizacji bieżącej estymaty rotacji zgodnie z zależnością: ˆθ m+1 ij = ˆθ ij m + δˆθ ij, gdzie m jest krokiem iteracji, a δˆθ ij wyznacza się ze wzoru: δˆθ ij = nij w którym jakobian J ma postać: k=1 (ui k ˆt ij ˆR ij u j k )T (P ij k ) 1 J ˆR ij u j k nij k=1 ( ˆR ij u j k )T JP 1 k J ˆR ij u j, (4.72) k J = [ ] 0 1. (4.73) 1 0 Skorygowana estymata ˆθ ij jest z kolei podstawiana do wzoru (4.70) i daje początek następnej iteracji algorytmu. Kroki, w których wykorzystuje się wzory (4.70) i (4.72), są wykonywane naprzemiennie do momentu wystąpienia warunku stopu proces jest zatrzymywany, gdy kolejne estymaty nie różnią się istotnie od siebie lub gdy zostanie osiągnięta założona maksymalna liczba iteracji (rys. 4.14). Najistotniejszą zaletą algorytmu WLSM w porównaniu z innymi metodami estymacji przemieszczenia robota na podstawie dopasowań typu punkt punkt jest uwzględnienie wag poszczególnych skojarzeń w procesie wyznaczania przemieszczenia. Występująca we wzorach (4.70 4.72) waga k-tej pary punktów należących do skanów wykonanych w pozycjach i oraz j jest macierzą kowariancji błędu dopasowania: P ij k = C P ij k + N P i k + R ij N P j k RT ij, (4.74) gdzie C P ij k to macierz kowariancji błędu wynikającego z kojarzenia punktów, które nie odpowiadają tym samym miejscom w otoczeniu, a N P i k i N P j k to macierze kowariancji określają- 185

TAK skan i skan j odczyt z odometrii NIE dopasowanie punktów warunek zakoñczenia estymacja przemieszczenia Rysunek 4.14. Schemat blokowy procedury dopasowywania skanów ce niepewność wynikającą z modelu sensora dla k-tej wiązki skanu wykonanego odpowiednio w i-tej i j-tej pozycji robota. Kowariancja związana z niepewnością pomiaru skanera laserowego jest funkcją mierzonych odległości rk i, rj k oraz kątów ϕi k, ϕj k. Błąd kojarzenia punktów zależy natomiast od kąta padania wiązki skanera na obserwowaną powierzchnię αk i oraz maksymalnych możliwych błędów dopasowania punktów. Błędy te są określone przez odległości pomiędzy sąsiadującymi punktami skanu. Dla punktu u i k i-tego skanu odległości te (rys. 4.12) dane są wzorami: δ i + = u i k+1 u i k, δ i = u i k u i k 1. (4.75) Macierz kowariancji określająca niepewność wynikającą z modelu sensora dla i-tego skanu dana jest wzorem: N P i k = (ri k )2 σϕ 2 [ 2sin 2 ϕ i k sin ] [ 2ϕi k 2 sin 2ϕ i k 2cos2 ϕ i + σ2 r 2cos 2 ϕ i k sin ] 2ϕi k k 2 sin 2ϕ i k 2sin2 ϕ i, (4.76) k gdzie σ 2 r jest wariancją pomiaru odległości skanera LMS 200, uzyskiwaną ze wzoru (2.85), a σ 2 ϕ wariancją kąta odchylenia wiązki skanera (2.86). Macierz kowariancji błędu dopasowania punktów C P ij k znajduje się natomiast na podstawie zależności: C P ij k = (δi +) 3 + (δ ) i 3 [ cos 2 η i 3(δ+ i + δ ) i k cos ηk i sin ηi k cos ηk i sinηi k sin 2 ηk i ], (4.77) gdzie ηk i = αi k +ϕi k. Macierz kowariancji (4.77) jest funkcją kąta padania k-tej wiązki skanera na obserwowaną powierzchnię. Ponieważ geometria otoczenia nie jest znana, kąt ten musi być określony na podstawie danych ze skanera. W pracy [234] kąt αk i wyznaczano za pomocą transformaty Hougha, która w danych pozwala znaleźć proste opisane równaniem (3.63). Na podstawie parametrów znalezionych prostych określa się kąty padania wiązek skanera na powierzchnie reprezentowane przez te proste. Z uwagi na koszt obliczeniowy transformaty Hougha (por. p. 3.23) oraz na to, że do określenia kąta αk i potrzebna jest jedynie lokalna aproksymacja geometrii powierzchni w punkcie padania k-tej wiązki, zdecydowano się zastosować inną metodę, będącą modyfikacją zaproponowanego wcześniej algorytmu grupowania punktów (algorytm 3.2). Schemat tej metody przedstawiono na rys. 4.13. Jeżeli k-ty odczyt odległości ma wartość r k, a poprzedni r k 1, to zakładając, że krzywizna obserwowanej powierzchni jest aproksymowana lokalnie linią prostą, można wyznaczyć kąt α k na podstawie znanych odległości r k i r k 1 oraz kąta ϕ między nimi. ctg α k = 1 sin ϕ ( rk r k 1 + cos ϕ 186 ). (4.78)

Ponieważ grupuje się pomiary wszystkich skanów, to kąt αk i dla wszystkich pomiarów danego (i-tego) skanu wyznacza się w pętli algorytmu 3.2, dzięki czemu operacja ta ma niewielki wpływ na koszt obliczeniowy przetwarzania skanu. Ostatnim etapem procedury dopasowywania skanów jest wyznaczenie macierzy kowariancji przemieszczenia robota, która ma postać: [ ] P ij Ptt P = tθ. (4.79) P θt P θθ Kowariancja wektora translacji P tt dana jest wzorem (4.71). Po wprowadzeniu oznaczenia: r T = n ij k=1 ( ˆR ij u j k )T J(P ij k ) 1 J ˆR ij u j k pozostałe składniki macierzy (4.79) można wyrazić wzorami: P θθ = 1, (4.80) r T P tθ = 1 r T ( nij ) 1 n ij (P ij k ) 1 k=1 k=1 ( ) (P ij k ) 1 JˆR ij u j k, (4.81) P θt = P T tθ. (4.82) Integracja z systemem EKF-SLAM Algorytm WLSM został zaimplementowany jako alternatywna forma odometrii dla systemu EKF-SLAM. W wyniku sumowania elementarnych przemieszczeń, danych w postaci estymat translacji ˆt i rotacji ˆθ, dla par następujących po sobie skanów można otrzymać alternatywną formę samolokalizacji zliczeniowej. Błąd pozycji robota estymowanej tą metodą może rosnąć w sposób nieograniczony, analogicznie jak w przypadku odometrii opartej na odczytach z enkoderów, ponieważ jednak dopasowywanie skanów opiera się na dokładnych pomiarach ze skanera laserowego, daje ono bardziej wiarygodne estymaty przemieszczenia oraz oszacowanie niepewności. Jednym z podstawowych założeń rozwiązania problemu SLAM jest statystyczna niezależność obserwacji (pomiarów) i sterowania (przemieszczeń robota), wyrażona wzorem (4.4). Z tego powodu w alternatywnej odometrii opartej na procedurze dopasowywania skanów nie można wykorzystywać tych samych pomiarów, które są następnie wykorzystywane jako podstawa wyodrębniania cech geometrycznych. W procedurze ekstrakcji cech ze wsparciem reprezentacji rastrowej bezpośrednio są wykorzystywane pomiary odległości z jednego skanu na dziesięć. Pozostałe służą jedynie do aktualizacji mapy rastrowej, a otrzymane cechy-odcinki są od nich statystycznie niezależne. W alternatywnej odometrii można więc wykorzystywać n u skanów wykonanych pomiędzy dwoma kolejnymi wywołaniami procedury ekstrakcji cech, a tym samym pomiędzy dwoma kolejnymi krokami EKF-SLAM. Procedura ta jest wykonywana niezależnie od odometrii zaimplementowanej w sterowniku robota. Krok algorytmu EKF-SLAM można zdefiniować jako k+1 = k+(n u +2)i, gdzie k jest numerem kroku SLAM, a i numerem skanu. W k-tym kroku estymatę pozycji robota ˆx R(k) otrzymuje się z EKF-SLAM (4.29). Po wykonaniu następnego skanu pozycję robota ˆx R(k+i) i jej kowariancję otrzymuje się na podstawie danych z odometrii robota, z użyciem formuł (2.24) i (2.25). Ponieważ skany wykonane w pozycji ˆx R(k+i) oraz następnej ˆx R(k+2i) nie są używane bezpośrednio w EKF-SLAM jako obserwacje, pozycja ˆx R(k+2i) może być wyznaczona metodą WLSM, która dostarcza lepsze estymaty niż odometria. Kolejna estymata 187

pozycji robota, będąca wynikiem przemieszczenia wyznaczonego przez dopasowanie skanów, jest obliczana z użyciem operacji złożenia transformacji [289]. Estymata pozycji robota oraz jej kowariancji po wykonaniu i + 1 skanu w sekwencji jest dana wzorami: ˆx i,i+1 cos ˆθ i ŷ i,i+1 sin ˆθ i + ˆx i ˆx R(i+1) = ˆx R(i) ˆx i,i+1 R = ˆx i,i+1 sin ˆθ i + ŷ i,i+1 cos ˆθ i + ŷ i, (4.83) ˆθ i + ˆθ i,i+1 C R(i+1) = J 1 C R(i) J T 1 + J 2 C J T 2, (4.84) gdzie jakobiany przyjmują postać: 1 0 ˆx i,i+1 sin ˆθ i ŷ i,i+1 cos ˆθ i cos ˆθ i sin ˆθ i 0 J 1 = 0 1 ˆx i,i+1 cos ˆθ i ŷ i,i+1 sin ˆθ i, J 2 = sin ˆθ i cos ˆθ i 0. (4.85) 0 0 1 0 0 1 Ostatnia aktualizacja estymaty pozycji w rozpatrywanej sekwencji, pomiędzy ˆx R(k+(nu+1)i) a ˆx R(k+1), także jest wykonywana na podstawie odczytów z enkoderów. Dzięki zastosowaniu powyższego schematu przetwarzania skanów do estymacji przemieszczenia robota i ekstrakcji cech geometrycznych używa się odrębnych odczytów odległości, a to pozwala spełnić warunek (4.4). Mimo że informacje pochodzące z odometrii opartej na odczytach enkoderów są dostępne w każdej chwili, nie są łączone za pomocą EKF z estymatą przemieszczenia robota otrzymaną z algorytmu WLSM. Jeżeli macierze kowariancji opisujące niepewność pomiarów otrzymanych z odometrii są nieadekwatne do stanu rzeczywistego, rezultaty takiej fuzji mogą być obarczone błędami podobnymi do błędów pozycji robota prognozowanej na podstawie danych z odometrii. Niezależnie obliczana przez sterownik pozycja robota jest odczytywana i wykorzystywana w sytuacjach, gdy podczas etapu dopasowywania nie udało się skojarzyć wystarczającej liczby punktów obu skanów lub gdy do wyznaczenia estymaty przemieszczenia za pomocą algorytmu WLSM nie wystarczyła maksymalna liczba iteracji. Oznacza to niepowodzenie procedury dopasowywania skanów i wówczas estymata przemieszczenia robota określana jest na podstawie odometrii. 4.4.4. Wyniki eksperymentów W celu zweryfikowania zaproponowanego nowego podejścia do realizacji metody EKF-SLAM wykonano kilka eksperymentów z użyciem robota TRC Labmate wyposażonego w skaner laserowy Sick LMS 200. Podobnie jak podczas testów oprogramowania opartego na podstawowej metodzie EKF-SLAM, eksperymenty wykonano w laboratorium oraz przyległej do niego części korytarza. Do oceny skuteczności ulepszeń wprowadzonych w LS/EKF-SLAM wykorzystano dane uzyskane podczas eksperymentu wykonanego w laboratorium, a opisanego w podrozdziale 4.3.4 (por. rys. 4.5). Odometrię robota TRC Labmate użytego w tym eksperymencie poddano procesowi kalibracji opisanemu w podrozdziale 2.2. Dzięki częściowemu przystosowaniu otoczenia w laboratorium, tj. przesłonięciu niektórych obszarów płytami styropianowymi oraz wprowadzeniu obiektów o regularnych kształtach (dwa kartony), otrzymano mapy wektorowe, których geometria dobrze nadaje się do oceny jakościowej i porównania. Na rysunku 4.15 przedstawiono mapy wektorowe uzyskane z użyciem standardowego systemu EKF-SLAM (rys. 4.15A) oraz systemu zmodyfikowanego zgodnie z koncepcją 188

A B Rysunek 4.15. Porównanie map otrzymanych podczas eksperymentu w laboratorium A B Rysunek 4.16. Widok 3D map otrzymanych podczas eksperymentu: A EKF-SLAM, B LS/EKF-SLAM LS/EKF-SLAM (rys. 4.15B). Mapa standardowego EKF-SLAM składa się z 78 cech geometrycznych, natomiast mapa powstała z użyciem LS/EKF-SLAM zawiera 31 takich cech. Zmniejszenie liczby cech nie oznacza jednak utraty szczegółowości odwzorowania geometrii A B Rysunek 4.17. Pomiary skanera LMS 200 nałożone na mapę na podstawie odometrii (A) i dopasowania skanów (B) 189

100 0.2 stopieñ dopasowania obserwacji [%] 90 80 70 60 50 40 30 20 10 LS/EKF-SLAM EKF-SLAM czas estymacji [s] 0.18 0.16 0.14 0.12 0.1 0.08 0.06 0.04 0.02 LS/EKF-SLAM EKF-SLAM 0 0 100 200 300 400 500 600 700 800 900 1000 przebyta odleg³oœæ [cm] 0 0 100 200 300 400 500 600 700 800 900 1000 przebyta odleg³oœæ [cm] Rysunek 4.18. Wyniki uzyskane podczas eksperymentu w laboratorium środowiska, gdyż nastąpiła redukcja cech zwielokrotnionych. Poprawa jakości mapy wektorowej osiągnięta w LS/EKF-SLAM jest wyraźnie widoczna w rzutach perspektywicznych (rys. 4.16). Redukcja liczby zwielokrotnionych cech geometrycznych świadczy o poprawie skuteczności dopasowywania cech w LS/EKF-SLAM, co można przypisać lepszej predykcji pozycji robota uzyskanej dzięki dopasowywaniu skanów (rys. 4.17). Brak na mapie wektorowej nieprawidłowych odcinków powstałych wskutek błędnej interpretacji niewielkich grup punktów wskazuje natomiast na lepsze odtwarzanie cech, wspomagane przez reprezentację rastrową. A B Rysunek 4.19. Skany nałożone na mapę na podstawie danych z odometrii (A) i dopasowania metodą WLSM (B) Na rysunku 4.18 porównano wyniki ilościowe uzyskane za pomocą EKF-SLAM i LS/EKF-SLAM. Liczba cech geometrycznych skojarzonych z mapą w kolejnych punktach 190

A B C Rysunek 4.20. Mapy uzyskane podczas eksperymentu w korytarzu (opis w tekście) ścieżki (rys. 4.18A) jest większa w systemie wykorzystującym dodatkowe informacje uzyskane ze skanera laserowego, co potwierdza poprawę skuteczności dopasowania za pomocą testu odległości Mahalanobisa. Czas integracji nowych obserwacji z mapą globalną (rys. 4.18B) wydłuża się w obu przypadkach wraz z przebytą drogą. W systemie LS/EKF-SLAM wydłużenie czasu estymacji następuje jednak wolniej ze względu na mniejszą liczbę cech na mapie. Wskazuje to, że skuteczne kojarzenie cech jest czynnikiem redukującym czas obliczeń w kolejnych krokach EKF-SLAM. Celem następnego eksperymentu było określenie odporności LS/EKF-SLAM na zakłócenia, których źródłem są obiekty dynamiczne. Na rysunku 4.19 pokazano wyniki korygowania błędów odometrii (szczególnie błędu orientacji) metodą dopasowywania skanów. Na rysunku 4.19A są widoczne skany nałożone na mapę na podstawie estymat pozycji uzyskanych z odometrii. Linią ciągłą zaznaczono ścieżkę nominalną, a linia przerywana łączy estymaty pozycji robota otrzymane na podstawie odczytów z enkoderów. Na rysunku 4.19B przedsta- 191

100 0.3 stopieñ dopasowania obserwacji [%] 90 80 70 60 50 40 30 20 10 LS/EKF-SLAM EKF-SLAM czas estymacji [s] 0.25 0.2 0.15 0.1 0.05 LS/EKF-SLAM EKF-SLAM 0 0 500 1000 1500 2000 2500 3000 przebyta odleg³oœæ [cm] 0 0 500 1000 1500 2000 2500 3000 przebyta odleg³oœæ [cm] Rysunek 4.21. Porównanie wyników uzyskanych podczas eksperymentu w korytarzu wiono te same dane nałożone na mapę na podstawie estymat pozycji wyznaczonych metodą dopasowywania skanów. Linia przerywana łączy estymaty pozycji uzyskane tą metodą. Na rysunku 4.20 pokazano, w jaki sposób usunięcie danych pochodzących od obiektów dynamicznych poprawia jakość mapy wektorowej. Na rysunku 4.20A przedstawiono mapę zbudowaną z surowych danych widocznych na rys. 4.19A. Zawiera ona wiele zbędnych cech-odcinków w obszarach, w których znajdowały się skupiska punktów pochodzących z pomiarów odległości do przemieszczającego się obiektu (osoby idącej w niewielkiej odległości przed robotem). Linią ciągłą zaznaczono ścieżkę utworzoną na podstawie odczytów z enkoderów, a linia przerywana przedstawia ścieżkę łączącą estymaty pozycji otrzymane z EKF-SLAM. Na rysunku 4.20B zamieszczono mapę rastrową FSG utworzoną podczas tego eksperymentu. Ciemniejszy kolor oznacza wyższy stopień przynależności do zbioru komórek wspierających hipotezę o istnieniu w danym miejscu stabilnej (stacjonarnej) cechy geometrycznej. Z kolei na rys. 4.20C przedstawiono mapę wektorową utworzoną przez system LS/EKF-SLAM, który za pomocą mapy rastrowej odfiltrowuje zakłócenia pochodzące od obiektów dynamicznych oraz dopasowuje skany w celu otrzymania lepszej estymaty przemieszczenia robota. Mapa ta zawiera mniej cech, a w szczególności nie ma na niej odcinków powstałych wskutek uwzględnienia danych pochodzących od obiektów dynamicznych. Wykonano także eksperyment ilustrujący możliwość tworzenia przez system LS/EKF-SLAM mapy wektorowej o zadowalającej jakości w sytuacji, gdy uzyskane za pomocą odometrii dane służące do predykcji pozycji robota są złej jakości. Na rysunkach 4.22 i 4.23 przedstawiono rezultaty eksperymentu wykonanego w pomieszczeniu laboratorium. Dane do tego eksperymentu zostały zebrane przed kalibracją błędów systematycznych odometrii robota TRC Labmate (por. p. 2.2), a parametry modelu niepewności odometrii zostały ustalone zgrubnie, na podstawie prostych doświadczeń [323]. Mapa przedstawiona na rys. 4.23A została utworzona przez zaimplementowany system SLAM (wersja podstawowa), w którym odczyty z enkoderów są wykorzystywane jako dane wejściowe na etapie predykcji EKF-SLAM. Struktury geometryczne otoczenia są na tej mapie reprezentowane w postaci bardzo wielu przecinających się lub nakładających odcinków. Mapa jest mocno zdeformowana, szczególnie jest to widoczne w lewej górnej części rys. 4.23A. Ponieważ robot zatoczył pętlę, w tym obszarze znajdują się zarówno cechy geometryczne umieszczone na mapie na początku eksperymentu, jak i te, które zostały dodane na jego końcu. Widoczne jest, że wraz z kolejnymi krokami EKF-SLAM zmniejszała się skuteczność dopasowywania obserwacji do cech geometrycznych, co w końcu doprowadziło 192

A B Rysunek 4.22. Skany nałożone na podstawie danych z odometrii (A) i mapa rastrowa utworzona na podstawie dopasowanych skanów (B) A B Rysunek 4.23. Porównanie map utworzonych podczas eksperymentu w laboratorium do dywergencji filtru i dużego błędu końcowej pozycji robota. Przyczyną tak mało efektywnego działania EKF-SLAM jest w tym przypadku niewłaściwe oszacowanie niepewności pozycji robota na etapie predykcji EKF, spowodowane błędem systematycznym w danych z odometrii (np. widoczne jest pogorszenie skuteczności dopasowania po wykonaniu obrotu w miejscu). Błędy prognozowanej pozycji robota prowadzą też do wzrostu błędu linearyzacji dokonywanej w otoczeniu estymaty pozycji (wzór 4.26). Wykres stosunku liczby dopasowanych obserwacji do liczby wszystkich cech geometrycznych wyodrębnionych w danym kroku (rys. 4.24A, linia przerywana) pokazuje, że opisane czynniki spowodowały stopniowy spadek skuteczności dopasowywania cech do poziomu uniemożliwiającego samolokalizację robota. Na rysunku 4.23B przedstawiono natomiast mapę wektorową utworzoną z użyciem systemu LS/EKF-SLAM, w którym wykorzystano dodatkowo dwa sensory logiczne, pozwalające 193

stopieñ dopasowania obserwacji [%] 100 90 80 70 60 50 40 30 20 10 A LS/EKF-SLAM EKF-SLAM 0 0 200 400 600 800 1000 1200 1400 1600 przebyta odleg³oœæ [cm] czas estymacji [s] 0.9 0.8 0.7 0.6 0.5 0.4 0.3 0.2 0.1 B LS/EKF-SLAM EKF-SLAM 0 0 200 400 600 800 1000 1200 1400 1600 przebyta odleg³oœæ [cm] Rysunek 4.24. Porównanie wyników uzyskanych za pomocą EKF-SLAM i LS/EKF-SLAM usunąć nieprawidłowe dane pochodzące od obiektów dynamicznych oraz wyznaczyć lepszą estymatę przemieszczenia robota. Widoczna jest istotna poprawa jakości mapy. Ponieważ za pomocą metody dopasowywania skanów otrzymano prawidłowe estymaty przemieszczenia robota niezależnie od odczytów z odometrii, nie wystąpiła tak duża dywergencja filtru i została zachowana dostateczna skuteczność dopasowywania cech, co jest widoczne na rys. 4.24A (linia ciągła). Niektóre struktury otoczenia nadal są reprezentowane przez zwielokrotnione odcinki, ale sumaryczna liczba cech geometrycznych na mapie uległa zmniejszeniu, co korzystnie wpłynęło na czas estymacji (rys. 4.24B). 4.4.5. Wnioski Na podstawie wyników eksperymentów przedstawionych w poprzednim podrozdziale można stwierdzić, że proponowana modyfikacja systemu jednoczesnej samolokalizacji i tworzenia mapy otoczenia pozwoliła na istotne polepszenie jakości otrzymywanych map wektorowych. Dodano procedury tworzenia mapy rastrowej FSG oraz dopasowywania skanów, które pozwalają na uzyskanie z pomiarów skanera laserowego dodatkowych informacji, niedostępnych w standardowym systemie EKF-SLAM. Dodatkowe informacje dotyczące stanu poszczególnych obszarów w funkcji czasu i przemieszczenia się robota pomiędzy kolejnymi punktami ścieżki, w których wykonywana jest procedura SLAM, umożliwiły: trafniejsze odtworzenie cech geometrycznych, a szczególnie eliminację niepewności co do przedmiotu pomiaru spowodowanej obecnością obiektów dynamicznych; poprawę predykcji pozycji robota w algorytmie EKF-SLAM. Dzięki obu tym czynnikom poprawiła się skuteczność kojarzenia obserwacji i cech ujętych już na mapie, mająca decydujące znaczenie dla poprawności działania EKF-SLAM. Zmniejszenie całkowitej liczby cech na mapie globalnej doprowadziło także do redukcji czasu wykonania etapu estymacji, co potencjalnie umożliwia tworzenie on-line map większych obszarów. W celu ulepszenia znanego algorytmu EKF-SLAM zidentyfikowano rodzaje i źródła niepewności, która ma krytyczne znaczenie dla działania tej metody. Następnie niepewność tę zredukowano za pomocą sensorów logicznych dodatkowych procedur przetwarzania danych pochodzących ze skanera laserowego. System LS/EKF-SLAM stanowi rozszerzenie 194

podstawowej implementacji EKF-SLAM i zachowuje jej struktury danych, metody predykcji i estymacji stanu oraz sposób dopasowywania cech oparty na indywidualnym testowaniu dopasowania poszczególnych obserwacji i na zasadzie najbliższego sąsiedztwa. Implementacja łącznego rozpatrywania dopasowania wszystkich obserwacji dokonanych w danym kroku [221] może przynieść dalszą poprawę jakości dopasowywania cech. Jest to temat kolejnych prac.

Rozdział 5 Architektura rozproszonego systemu robotów i sensorów stacjonarnych 5.1. Wprowadzenie W poprzednich rozdziałach przedstawiono propozycje metod przetwarzania i gromadzenia danych otrzymanych z sensorów robota mobilnego. Wykazano eksperymentalnie, że metody te charakteryzują się większą odpornością na różne rodzaje niepewności tych danych. Odporność tę osiągnięto jednak po części kosztem komplikacji procesu przetwarzania danych, który stał się procesem wieloetapowym, opartym na reprezentacjach pośrednich (mapach) powiązanych przepływem strumieni danych o rosnącym stopniu abstrakcji. W proponowanych rozwiązaniach można wyróżnić odrębne moduły, mogące działać niezależnie, ale także mogące współpracować ze sobą. Przykładem są tutaj procedury tworzenia mapy rastrowej (por. p. 3.2.3) i mapy wektorowej (por. p. 3.3). Mapy te stanowią alternatywne formy reprezentacji informacji przestrzennej o środowisku, jednak jak wykazano w podrozdziale 3.4.4, mogą być użyte wspólnie, dzięki czemu kompensują swoje podstawowe wady. Jeżeli opisane w niniejszej pracy procedury przetwarzania danych są używane jako moduły złożonego systemu nawigacji na podstawie danych uzyskanych z sensorów, to dodatkowo należy uwzględnić zależności czasowe między poszczególnymi modułami, ponieważ informacja z sensorów pełni swoją funkcję jedynie wówczas, gdy jest przetwarzana i udostępniana innym modułom w odpowiednim czasie. Złożoność systemu przetwarzania i gromadzenia danych uzyskanych z sensorów zwiększa się, gdy robot korzysta z zewnętrznych źródeł informacji, takich jak kamery monitorujące (przedstawione w p. 2.5) lub systemy percepcyjne innych robotów, wyposażonych w odmienne konfiguracje sensorów. Możliwości takie mają roboty działające w ramach rozproszonego systemu robotów i sensorów stacjonarnych, który powstał w Laboratorium Robotów Mobilnych IAiII [282]. Powyższe rozważania można podsumować stwierdzeniem, że w proponowanym rozwiązaniu zbieranie i przetwarzanie informacji o środowisku odbywa się w ramach wielu współbieżnych procesów percepcji i interpretacji danych. Zagadnienie efektywnej percepcji i modelowania otoczenia w takim systemie można przedstawić jako problem zorganizowania 196

współpracy poszczególnych modułów. Do tego potrzebna jest odpowiednia architektura systemu nawigacji robota mobilnego. Przez architekturę rozumie się tu specyfikację modułów funkcjonalnych, struktur danych, na których moduły te działają, oraz powiązań między nimi. Architektura robota powinna być częścią architektury oprogramowania całego systemu rozproszonego. Pozwala to efektywnie wykorzystać nowe możliwości, które stwarza systemowi percepcyjnemu pojedynczego robota praca w ramach zespołu. Architektura systemu rozproszonego powinna obejmować także specyfikację mechanizmów wymiany informacji pomiędzy jego komponentami. Systemy wielorobotowe są obecnie w centrum zainteresowania wielu ośrodków badawczych [233]. Analizując dostępne publikacje, można jednak stwierdzić, że niewiele prac poświęcono rozproszonej percepcji i kooperatywnemu modelowaniu otoczenia. Badania w dziedzinie systemów rozproszonych w robotyce mobilnej koncentrują się w większości na takich aspektach tych systemów, jak: budowa uporządkowanej formacji robotów i sterowanie ruchem takiej formacji [19], alokacja zadań w grupie robotów [9, 203, 215, 232], koordynacja działań lub zachowań w zespole [91, 322]. Zagadnienia przetwarzania, integracji i wymiany informacji pochodzącej z różnych sensorów działających w rozproszonych systemach robotycznych są poruszane w literaturze znacznie rzadziej. W pracach dotyczących architektury systemów wielorobotowych oraz zagadnień koordynacji i kooperacji często przyjmuje się założenie o symbolicznym charakterze percepcji [215], a do weryfikacji proponowanych rozwiązań wykorzystuje się środowiska symulacyjne [29, 44, 55, 91, 107]. W wielu zespołach współpracujących fizycznych robotów mobilnych wykorzystuje się sterowanie behawioralne, które nie wymaga modelu otoczenia [58] lub opiera się na modelu danym a priori (np. topologicznym [9] lub wektorowym [24]). Także działanie budowanych i badanych obecnie w wielu ośrodkach zespołów robotów grających w piłkę nożną (ang. robot soccer) jest oparte na sensorach zapewniających globalną wiedzę o środowisku (kamera nad polem gry) [91] lub na właściwościach szczególnego otoczenia, jakim jest boisko zgodne z regułami rozgrywek RoboCup. Podjęta w niniejszym rozdziale próba zdefiniowania dla robotów mobilnych i wspierających je w zadaniach nawigacyjnych sensorów stacjonarnych (kamer monitorujących) architektury umożliwiającej efektywną realizację zadań percepcji i budowy modelu otoczenia obejmuje więc nowy i stosunkowo mało zbadany obszar. Proponowana architektura jest oparta na opisanych w niniejszej pracy modułach funkcjonalnych, przetwarzających dane sensoryczne z uwzględnieniem redukcji ich niepewności, i odwołuje się do koncepcji i rozwiązań strukturalnych wprowadzonych we wcześniejszych pracach: agentowej architektury tablicowej (ang. Multi-Agent Blackboard MAB) [63] i mechanizmu komunikacji między agentami opartego na protokole kontraktacyjnym [155]. W pracach [149, 150, 152] zaproponowano wykorzystanie koncepcji sieci percepcyjnej (ang. perception network) [179] jako nowego podejścia do wymiany i łączenia informacji z wielu sensorów. Sieć ta reprezentuje przepływ informacji od źródeł (sensorów) do modeli otoczenia, wykorzystywanych następnie przez robota w zadaniach nawigacyjnych (samolokalizacji, planowania ścieżki). Wymiana danych w sieci prowadzona jest w celu zmniejszenia ich niepewności i przedstawienia zgromadzonej informacji w postaci dogodnej do pełnienia poszczególnych funkcji nawigacyjnych robota. Redukcja niepewności możliwa jest dzięki porównywaniu i łączeniu modeli budowanych na podstawie danych pochodzących z poszczególnych sensorów i obserwacji poczynionych w różnych punktach (pozycjach robota) [99, 189]. Zaproponowano wprowadzenie do struktury sieci, jako jej podstawowych wierzchołków, map otoczenia robota oraz jednoczesne wykorzystanie zróżnicowanych mo- 197

deli niepewności informacji. Jednocześnie zaproponowano implementację sieci percepcyjnej jako systemu wieloagentowego [152]. Argumentem za przyjęciem do łączenia informacji ujęcia wieloagentowego zamiast podejścia klasycznego (hierarchicznego) [102, 167, 318] była chęć uzyskania takich właściwości charakterystycznych dla agentowego modelu systemów, jak otwartość, skalowalność oraz zdolność tolerowania lokalnych uszkodzeń i współdzielenia zasobów [244]. W toku dalszych prac propozycję sieci percepcyjnej dla zespołu robotów mobilnych przekształcono w wieloagentową architekturę rozproszonego systemu robotów i sensorów stacjonarnych. W szczególności wprowadzono agentową, tablicową architekturę oprogramowania robota mobilnego [62, 63]. Struktura tego rozwiązania opiera się na modułach funkcjonalnych, rozpoznanych i opisanych wcześniej w postaci sieci percepcyjnej [150], a koncepcję zastosowania architektury tablicowej do robota wykonującego zadanie modelowania otoczenia nakreślono po raz pierwszy w artykule [59]. 5.2. Wieloagentowy system robotów i sensorów stacjonarnych Roboty i stacjonarne kamery monitorujące współdziałają ze sobą, tworząc rozproszony system wielosensoryczny, którego zadaniem jest obserwacja środowiska i budowa (aktualizacja) jego modelu [152, 264]. Przetwarzanie i gromadzenie danych uzyskanych z sensorów prowadzone jest przez niezależne, współbieżne procesy percepcji. Procesy te wykonywane są przez roboty mobilne ze zróżnicowanymi systemami sensorycznymi oraz sensory stacjonarne, traktowane jako odrębne komponenty. Liczba i właściwości komponentów mogą ulegać zmianom, dzięki czemu system jest podatny na modyfikacje. Wśród możliwych praktycznych zastosowań systemu wielorobotowego o takich właściwościach należy wymienić sferę usług [24], patrolowanie i ochronę rozległych obiektów [105], transport wewnątrzzakładowy w przemyśle [135, 171] i inne zautomatyzowane systemy transportowe [9]. Zakłada się, że roboty mobilne wchodzące w skład systemu wykonują przydzielone im wcześniej zadania, polegające na przebyciu określonej trasy, bez fizycznej interakcji między robotami (np. wspólnego przemieszczania przedmiotów). Proces alokacji zadań do poszczególnych robotów, będący przedmiotem wielu prac badawczych [9, 10, 215], nie jest tu rozważany. Główna motywacja badań dotyczących wieloagentowych systemów robotów wynika z intuicyjnego przekonania, że zespół zróżnicowanych, kooperujących ze sobą komponentów może wykonać określone zadanie efektywniej niż pojedynczy robot [96]. Architektura systemu jest oparta na metodyce wieloagentowej. W piśmiennictwie z dziedziny robotyki i informatyki występują różne definicje pojęcia agenta i systemu wieloagentowego [109]. W zastosowaniach z zakresu robotyki agentami są określone struktury programowe (ang. software agents) lub programowo-sprzętowe. Agenci mający fizyczną część wykonawczą (sensor, efektor, robot) zwani są agentami upostaciowionymi (ang. embodied agents). Agent ma zdolność percepcji i komunikowania się z otoczeniem, a jego funkcjonalność wyraża się w wykonywanych akcjach, w tym akcjach komunikacyjnych [215]. System wieloagentowy to zbiór agentów. Cechy agenta to autonomia, reaktywność i celowość działania. Autonomia rozumiana jest w tym kontekście jako zdolność podejmowania decyzji i akcji na podstawie percepcji i/lub wewnętrznego modelu świata, w którym działa agent. Reaktywność oznacza zdolność do reagowania na bodźce odbierane ze środowiska, w tym również komunikaty. Przez celo- 198

wość działania, zwaną także proaktywnością (ang. proactivity), rozumiane jest podejmowanie działań, których źródłem jest imperatyw wewnętrzny, określony jako cel działania (istnienia) agenta. W zależności od przyjętej koncepcji systemu sterowania agent upostaciowiony może być sklasyfikowany jako reaktywny lub deliberatywny, lecz współczesne architektury robotów często łączą w jednym systemie elementy różnych koncepcji sterowania, co prowadzi do rozwiązań hybrydowych [18, 167, 217]. Ujednoliconą, formalną koncepcję opisu sposobu sterowania robotami/agentami w systemach wieloagentowych zaproponowano w pracy [331]. Koncepcja ta może służyć do funkcjonalnej specyfikacji zachowań agentów behawioralnych, deliberatywnych i hybrydowych. Architektury rozproszonych systemów robotów mobilnych można usystematyzować, biorąc pod uwagę różne kryteria. Próby takiej klasyfikacji podjęto w pracach [96, 294]. W szczególności kooperatywne systemy robotów mobilnych można podzielić na dwa rodzaje ze względu na sposób współpracy i komunikacji między robotami. Są to: systemy oparte na koncepcji roju (ang. swarm intelligence), systemy oparte na świadomej kooperacji jednostek (ang. intentional co-operation). Systemy kooperatywne o strukturze roju składają się na ogół z licznych, stosunkowo prostych, jednorodnych robotów, których prymitywne zachowania indywidualne w wyniku interakcji generują bardziej złożone formy zachowania grupowego [202]. Inspiracją badań są tu głównie rezultaty uzyskane w biologii. Sterowanie tego rodzaju zespołami robotów jest z reguły oparte na paradygmacie behawioralnym [18], a koordynacja działań poszczególnych jednostek, prowadząca do osiągnięcia nadrzędnego celu wytyczonego dla całej grupy, odbywa się dzięki komunikacji poprzez środowisko (ang. implicit communication), czyli przez obserwację efektów działań innych członków grupy [96]. Kooperacja uświadomiona wymaga użycia bardziej złożonych agentów działających na rzecz wykonania określonego wspólnego zadania. Sterowanie agentami może mieć charakter behawioralny lub hybrydowy. Najczęściej podstawą kooperacji jest tu jawnie sformułowany cel działania oraz wspólny plan. W takim przypadku kluczem do uzyskania spójnego działania agentów są ich możliwości percepcyjne oraz możliwość wzajemnego komunikowania się [29]. Komunikacja ma charakter jawnej wymiany wiadomości między agentami (ang. explicit communication). Powstały w IAiII system składa się ze stosunkowo niewielkiej liczby komponentów, różniących się właściwościami, a jego podstawowym zadaniem jest uzyskiwanie informacji przydatnych w nawigacji. Stąd też odpowiednim dla niego modelem współpracy jest świadoma kooperacja oparta na wzajemnym, jawnym komunikowaniu się. Ze względu na znaczną złożoność procesów przetwarzania informacji wewnętrzna architektura robotów powinna być hybrydowa z komponentami deliberatywnymi, odpowiedzialnymi za modelowanie, planowanie i kooperację, oraz warstwą odruchową, odpowiedzialną za sterowanie pojazdem [215]. Do opisu systemu, którego komponenty operują autonomicznie i mogą się komunikować ze sobą, wykorzystano pojęcie agenta. W rozważanym systemie zdefiniowano trzy klasy agentów. Są to: agenci roboty, agenci percepcyjni związani z sensorami stacjonarnymi oraz agent operator, stanowiący punkt styku systemu autonomicznego z jego nadzorcą człowiekiem. Zadania percepcji i tworzenia modelu otoczenia wykonują agenci roboty (ang. robot agent RA) poruszający się w wybranym środowisku (laboratorium). Są to podstawowe jednost- 199

AGENT PERCEPCYJNY IEEE 802.3 AGENT PERCEPCYJNY AGENT OPERATOR (interfejs u ytkownika) kamera IEEE 802.11b kamera Robot_A Robot_B LED LED AGENT ROBOT AGENT ROBOT Rysunek 5.1. Struktura rozproszonego systemu robotów i sensorów stacjonarnych ki systemu, które oprócz zbierania danych o otoczeniu mogą wykonywać wybrane zadania użytkowe, np. transportować obiekty do określonych miejsc lub patrolować określone trasy. Agenci percepcyjni (ang. perception agent PA) reprezentują odrębne podsystemy dysponujące sensorami i modelem otoczenia lub wiedzą o jego elementach (np. o pozycji agenta robota). Agenci percepcyjni są zdefiniowani jako źródła informacji składające się z fizycznego źródła danych (sensora) oraz procedur przetwarzania danych, używanych do wyodrębniania użytecznej informacji z surowych danych otrzymanych z sensora fizycznego (np. określania pozycji robota na podstawie obrazu). Ponieważ agenci percepcyjni nie mają fizycznych efektorów, podejmowane przez nich akcje ograniczają się do komunikacji. W zbudowanym systemie demonstracyjnym fizyczną bazą agentów percepcyjnych są komputery stacjonarne współpracujące z kamerami monitorującymi [264]. Agent operator (ang. operator agent OA) inicjuje działanie systemu i konfiguruje jego składniki agentów roboty i agentów percepcyjnych. Wykorzystując odpowiedni protokół komunikacyjny, program operatora ustala wybrane parametry konfiguracyjne pozostałych agentów, co umożliwia im automatyczne dołączenie do działającego systemu. Agent operator zapewnia też prosty interfejs użytkownika. W przeciwieństwie do pozostałych agentów agent operator nie jest związany z konkretnymi zasobami sprzętowymi i może być uruchomiony na dowolnym komputerze włączonym w sieć lokalną, w której działa system. Z punktu widzenia architektury systemu rozproszonego, w którym agentami są struktury programowo- -sprzętowe o określonej funkcjonalności, program konfiguracyjno-nadzorczy nie musi być odrębnym agentem. Potraktowanie tego programu jako agenta pozwoliło jednak ujednolicić mechanizm komunikacji w systemie [281]. Na rysunku 5.1 pokazano opisaną architekturę systemu w konfiguracji odpowiadającej stanowisku badawczemu w Laboratorium Robotów Mobilnych [264]. W skład tego systemu wchodzą dwa roboty mobilne typu Labmate, wyposażone w zróżnicowane zestawy sensorów, oraz dwie kamery monitorujące. 200

5.3. Architektura agenta robota 5.3.1. Analiza procesu przetwarzania informacji przez robota mobilnego W autonomicznym robocie mobilnym zachodzi złożony proces przetwarzania danych, uzyskanych z sensorów zewnętrznych i wewnętrznych, w modele użyteczne podczas wykonywania poszczególnych funkcji systemu nawigacji robota, takich jak samolokalizacja i planowanie ruchu. W dalszej części niniejszego podrozdziału jako przykład będzie rozważany robot TRC Labmate, wyposażony w skaner Sick LMS 200, dalmierze ultradźwiękowe i kamerę pokładową. Oprogramowanie tego robota, w którym zaimplementowano metody przetwarzania i gromadzenia informacji z sensorów opisane w rozdziale 3 oraz system SLAM przedstawiony w podrozdziale 4.4, wykonuje następujące operacje związane z przetwarzaniem danych i sterowaniem pojazdem: wyznaczanie estymaty pozycji robota i niepewności tej pozycji za pomocą podsystemu samolokalizacji zliczeniowej (odometrii); aktualizacja mapy rastrowej na podstawie danych ze skanera laserowego; aktualizacja mapy rastrowej na podstawie danych z dalmierzy ultradźwiękowych; analiza mapy rastrowej i wyznaczanie kolejnego ruchu robota (sterowanie); śledzenie pozycji robota z wykorzystaniem metody dopasowywania skanów; wyodrębnianie cech geometrycznych na podstawie danych ze skanera tworzenie lokalnej mapy wektorowej; realizacja algorytmu EKF-SLAM jednoczesne tworzenie globalnej mapy wektorowej i wyznaczanie pozycji robota; wyznaczanie pozycji robota i jej niepewności za pomocą pokładowego podsystemu wizyjnego na podstawie obserwacji sztucznych znaczników; wyznaczanie aktualnej pozycji robota i jej niepewności na podstawie dostępnych niezależnych estymat pozycji uzyskanych z systemu EKF-SLAM lub odometrii oraz pokładowego podsystemu wizyjnego. Jeżeli dany robot współpracuje z innymi agentami robotami i sensorami stacjonarnymi w ramach systemu wieloagentowego, to lista zadań przetwarzania informacji z sensorów poszerza się o: wyznaczanie aktualnej pozycji robota i jej niepewności w wyniku integracji estymaty pozycji dostępnej dla robota (odometria, EKF-SLAM, kamera pokładowa) i estymaty otrzymanej ze źródła zewnętrznego (od agenta percepcyjnego lub innego robota); aktualizację globalnej mapy wektorowej w wyniku zintegrowania jej z danymi z lokalnej mapy wektorowej innego robota. 201

Wszystkie powyższe operacje są dobrze zdefiniowanymi podzadaniami, a każde z nich można postrzegać jako odrębną czarną skrzynkę o określonym wejściu i wyjściu. Operacje te są ze sobą luźno powiązane przekazywanymi między nimi danymi. Postać danych przekazywanych pomiędzy poszczególnymi procedurami także jest dobrze określona i nie zależy od znaczenia poszczególnych czarnych skrzynek. Porządek wykonywania operacji nie może być jednak ustalony a priori, gdyż zależy on od bieżących wartości danych, np. nieprawidłowe działanie EKF-SLAM może być bodźcem do zainicjowania procedury pozycjonowania przez zewnętrznego agenta kamerę monitorującą. Poszczególne operacje wywoływane są w sposób oportunistyczny, a cały proces przetwarzania jest sterowany danymi. Taki system można rozpatrywać jako system tablicowy. Architektura tablicowa jest analogią sposobu działania zespołu ekspertów, wspólnie rozwiązujących pewien problem i porozumiewających się przez zapisywanie wyników swoich działań na tablicy (ang. blackboard) dostępnej dla wszystkich uczestniczących w dyskusji. Jest ona chętnie stosowana w oprogramowaniu systemów eksperckich [42], znane są także przykłady wykorzystania tego podejścia w robotyce [146]. System tablicowy składa się z tablicy, zbioru modułów przetwarzających dane (ekspertów) oraz mechanizmu sterowania. Tablica jest złożoną, wielodostępną strukturą danych, do której poszczególne moduły przesyłają wyniki swojego przetwarzania, a część z nich pobiera też dane. Pozostała część modułów pobiera dane z sensorów fizycznych. Surowe dane sensoryczne nie są wprowadzane do tablicy, są przetwarzane przez odpowiednie moduły programowe obsługujące sensory. By zunifikować dostęp do danych, wykorzystano koncepcję sensora logicznego [128]. Pozwala to niezależnie interpretować istotnie odmienne cechy środowiska, postrzegane przez dany sensor fizyczny (np. cechy geometryczne i stan zajętości obszaru postrzegane przez skaner laserowy). Moduły otaczające tablicę i mające dostęp do sensorów fizycznych są funkcjonalnymi odpowiednikami agentów sensorycznych proponowanych w pracach [149, 152]. Mechanizm sterowania odpowiada za aktywację poszczególnych modułów i realizację kolejnych cykli działania całego systemu. System tablicowy można modelować jako system agentowy [257]. Umieszczone wokół tablicy moduły przetwarzające oraz sensory dostarczające informacje są agentami, a w celu odróżnienia ich od agentów stanowiących komponenty całego systemu wielorobotowego (agent robot, agent percepcyjny) będą one dalej nazywane także agentami tablicowymi (ang. blackboard agents). Wewnętrzna struktura agentów tablicowych może być prosta, gdyż mają oni ściśle określone zadania (np. ekstrakcja cech, aktualizacja mapy), które wykonują bez odwoływania się do innych agentów, a środowiskiem ich działania jest tablica, z której pobierają informacje i na której zapisują wyniki swoich działań [264]. Na środowisko działania agenta tablicowego składają się: tablica i zbiór pozostałych agentów. Agent może wykonywać swoje akcje równolegle z akcjami innych agentów jest implementowany w postaci wątku. Synchronizacja z innymi agentami korzystającymi z tych samych zasobów odbywa się za pomocą środków programowych udostępnianych przez system operacyjny [114, 121]. Agenci zgromadzeni wokół tablicy komunikują się wyłącznie za jej pomocą. Aktywność danego agenta jest inicjowana przez pojawienie się lub zmianę danych na tablicy. Poszczególni agenci obserwują jedynie zmiany w wybranych strukturach danych. Wykorzystanie podejścia agentowego do budowy oprogramowania sprzyja jego modularności i skłania do zastosowania programowania obiektowego [61]. Zaletą agentowego systemu tablicowego jest także podatność na modyfikacje, gdyż w prosty sposób można dołączać lub usuwać poszczególnych agentów (co jest ważne w przypadku zmiany zestawu sensorów 202

danego robota). Przy tym samym zestawie agentów można modyfikować proces przekształcania danych sensorycznych, zmieniając sterowanie w systemie. 5.3.2. Proponowana struktura tablicy i zestaw agentów Na tablicy umieszczone są dane, które reprezentują położenie i orientację robota oraz opisują jego otoczenie na różnych poziomach abstrakcji. Cechy geometryczne wyodrębnione z danych uzyskanych ze skanera laserowego zawiera lokalna mapa wektorowa. Stan środowiska bezpośrednio otaczającego robota odzwierciedla mapa rastrowa, uaktualniana na podstawie kolejnych odczytów sensorów odległości. Informacje z obu tych map są wykorzystywane do budowy w wyższym stopniu przetworzonego modelu otoczenia, którym jest globalna mapa wektorowa. Na tablicy znajdują się także kolejki komunikatów wymienianych z innymi agentami, podstawowe informacje statusowe dotyczące działania poszczególnych sensorów i modułów programowych oraz informacje (flagi) wykorzystywane do sterowania w systemie tablicowym [64]. Zestaw agentów w tablicowej architekturze robota mobilnego został ustalony w oparciu o zbiór operacji wykonywanych przez autonomicznego robota mobilnego. Dodatkowym kryterium wyodrębnienia agentów urządzeń była kontrola danego urządzeniea fizycznego (np. sensora, sterownika) przez pojedynczego agenta. Zestaw ten pozwala wykonać zadanie przetwarzania i gromadzenia danych sensorycznych, nie umożliwia jednak wykonania wszystkich zadań, które może podejmować robot autonomiczny, a w szczególności nie zawarto w nim modułów przeznaczonych do strategicznego planowania działań i ruchów robota (eksploracja, planowanie ścieżki) oraz funkcji zapewniających efektywną kooperację w systemie wielorobotowym (budowa i utrzymanie formacji, synchronizacja planów działania). Zagadnienia te wykraczają poza tematykę niniejszej pracy, stanowią jednak przedmiot wielu badań dotyczących systemów wieloagentowych w robotyce, a propozycje rozwiązań w tym zakresie można znaleźć między innymi w pracach [10, 44, 215]. Architektura tablicowa nie stanowi ograniczenia dla wprowadzenia tego rodzaju funkcji do oprogramowania robota, gdyż można rozbudowywać strukturę tablicy oraz rozszerzać zestaw agentów. Biorąc pod uwagę powyższe czynniki oraz przedstawioną uprzednio analizę procesu przetwarzania informacji przez robota, zaproponowano strukturę tablicy agenta robota oraz zestaw jego agentów tablicowych. Na tablicy agenta robota dostępne są następujące dane: mapa rastrowa utworzona metodą zbiorów rozmytych; lokalna mapa wektorowa utworzona z wykorzystaniem informacji z mapy rastrowej (FSG); globalna mapa wektorowa utworzona za pomocą algorytmu EKF-SLAM; estymata pozycji robota otrzymana z odometrii; estymata pozycji robota otrzymana z EKF-SLAM; estymata pozycji robota otrzymana z pokładowego systemu wizyjnego; aktualna estymata pozycji robota (otrzymana w wyniku połączenia danych); dane znane a priori: 203

lista landmarków dla systemu wizyjnego, ścieżka (zaplanowana off-line); informacje statusowe i sterujące. Ze sprzętowymi komponentami robota są związani następujący agenci urządzeń: ADScan agent skanera laserowego. Obsługuje skaner i wstępnie przetwarza dane oraz określa ich niepewność. Zapisuje status skanera (sprawny/niesprawny). Działanie podejmuje co ustalony kwant czasu. ADSonar agent dalmierzy ultradźwiękowych. Obsługuje zestaw sonarów, wstępnie przetwarza dane i określa ich niepewność. Zapisuje status sonarów. Działanie podejmuje co ustalony kwant czasu. ADCam agent pokładowego systemu wizyjnego. Działanie tego agenta jest inicjowane tylko w związku z koniecznością określenia pozycji robota. Analizuje on obraz, wykrywa znaczniki, porównuje je z dostępną listą znaczników i wyznacza na tej podstawie estymatę pozycji. Odnotowuje stan systemu wizyjnego (sprawny/niesprawny). ADCtrl agent sterownika robota co ustalony kwant czasu umieszcza na tablicy wyniki działania podsystemu odometrii. Do jego zadań należy też wysyłanie do sterownika robota rozkazów ruchu wyznaczonych przez agenta AEPilot. Zapisuje na tablicy status sterownika robota. ADComm agent komunikacji. Obsługuje fizyczny kanał komunikacyjny. Jego działanie jest inicjowane przez obecność komunikatów w kolejce komunikatów do wysłania (OutMsg) lub po nadejściu komunikatu od innego agenta. W pierwszym przypadku komunikaty są pobierane z kolejki i wysyłane, a w drugim odbierane i dołączane do kolejki InMsg. Przetwarzaniem informacji uzyskanej z sensorów zajmują się agenci eksperci: AEPilot agent lokalnego planowania ruchu robota. Wykonuje reaktywne funkcje systemu sterowania robota mobilnego (unikanie kolizji, omijanie przeszkód). W konkretnej implementacji [264] jako metodę planowania ruchu robota zastosowano odmianę znanego algorytmu VFH [48]. Agent AEPilot cyklicznie analizuje mapę rastrową, wykrywa przeszkody i określa kolejny ruch robota. Ma dostęp do zaplanowanej ścieżki oraz aktualnej pozycji robota. AEFuzzGrid agent aktualizacji mapy rastrowej. Implementuje rozmytą metodę tworzenia mapy rastrowej, przedstawioną w podrozdziale 3.2.3. Tworzy mapę FSG oraz wyznacza zbiory komórek przydatnych z punktu widzenia funkcji planowania ruchu [229], a wykorzystywanych przez agenta AEPilot. Warunkiem rozpoczęcia działania jest pojawienie się nowych danych ze skanera lub sonarów. AEFeatExt agent ekstrakcji cech geometrycznych. Działanie tego agenta inicjowane jest w ustalonych odstępach czasu, gdy trzeba wyznaczyć cechy geometryczne na podstawie bieżącego skanu. Implementuje on metodę wyodrębniania odcinków ze wsparciem reprezentacji rastrowej, opisaną w podrozdziale 3.4.4. Zapisuje na tablicy lokalną mapę wektorową. 204

ZMIENNE STATUSOWE AEReport AEMapUpd ADComm ADCam OutMsg InMsg EST. POZYCJI Z KAMERY MAPA GLOBALNA AEPosUpd ADSonar ADScan BLACKBOARD DANE Z SONARÓW DANE ZE SKANERA POZYCJA ROBOTA MAPA LOKALNA MAPA RASTROWA AESlamAlg AEFeatExt AEFuzzGrid ADCtrl EST. POZYCJI Z ODOMETRII OPIS ZADANIA POLECENIA AEScnMatch AEPilot Rysunek 5.2. Architektura agenta robota AEScnMatch agent dopasowywania skanów. Implementuje metodę dopasowywania skanów opisaną w podrozdziale 4.4. Warunkiem rozpoczęcia działania jest pojawienie się nowych danych ze skanera. Zapisuje na tablicy estymatę pozycji robota wraz z niepewnością. AESlamAlg agent EKF-SLAM. Jego zadaniem jest realizacja algorytmu EKF-SLAM w postaci opisanej w podrozdziale 4.4 (LS/EKF-SLAM). Działanie tego agenta inicjowane jest w ustalonych odstępach czasu lub przez agentów AEMapUpd i AEPosUpd. Warunkiem uruchomienia jest dostępność nowej prognozy pozycji uzyskanej na podstawie dopasowywania skanów lub odometrii. Na tablicy uaktualnia mapę globalną i pozycję robota wraz z niepewnością (wektor stanu EKF-SLAM). AEPosUpd agent pozycjonowania. Cyklicznie odczytuje estymatę pozycji robota i ustala konieczność pozycjonowania robota w oparciu o ustalenia pokładowego systemu wizyjnego (ustawia odpowiedni znacznik), a w dalszej kolejności sięga do informacji od zewnętrznych agentów i dołącza odpowiedni komunikat do kolejki OutMsg. Aktualizacja pozycji robota na podstawie bieżącej estymaty oraz informacji uzyskanej ze źródeł zewnętrznych dokonywana jest za pomocą statycznego filtru Kalmana. Agent pozycjonowania zapisuje aktualną pozycję robota na tablicy. Inicjuje AESlamAlg, który dokonuje korekcji stanu systemu (por. wzór 4.23). AEMapUpd agent aktualizacji mapy globalnej na podstawie informacji zewnętrznych. Dokonuje transformacji mapy lokalnej otrzymanej od innego agenta robota i uruchamia agenta tablicowego AESlamAlg, który integruje te dane z mapą globalną. Warunkiem 205

rozpoczęcia aktualizacji globalnego modelu otoczenia jest odebranie komunikatu od zewnętrznego agenta. Wykrywa także sytuacje, w których robot może pobrać dane do modelu otoczenia od innego agenta robota i wówczas dołącza odpowiedni komunikat do kolejki komunikatów OutMsg. AEReport agent sprawozdawca. Jego zadanie polega na przygotowywaniu komunikatów o stanie robota dla operatora systemu (dołączanie ich do kolejki OutMsg). Ma on dostąp do całej tablicy. Działanie podejmuje co ustalony kwant czasu w przypadku wystąpienia awarii któregoś z podsystemów (odpowiednie wartości danych statusowych) lub na żądanie operatora (pojawienie się komunikatu od operatora w kolejce InMsg). Na rysunku 5.2 przedstawiono proponowaną strukturę agenta robota dla robota TRC Labmate wykorzystywanego w eksperymentach przedstawionych w podrozdziałach 4.4, 5.6 i 6.4. Rozszerzenie proponowanego systemu o elementy planujące nie wymaga większych zmian jedynie rozszerzenia zestawu agentów wokół tablicy. Podsystem planujący dla agenta robota składa się z planera wytyczającego możliwe drogi przejazdu (ang. route planner) oraz planera optymalnej ścieżki (ang. path planner). W przypadku braku elementów planujących ścieżka jest dostarczana do agenta robota w momencie jego inicjacji i pozostaje niezmienna przez cały czas wykonywania zadania. Należy jednak zauważyć, że wprowadzenie planowania ścieżki przez agentów nie rozwiązuje wszystkich problemów koordynacji i zapobiegania blokadom w systemie wieloagentowym [10, 44]. 5.3.3. Realizacja EKF-SLAM we współpracy z innymi robotami Algorytm EKF-SLAM na etapie estymacji (por. wzory 4.28 4.30) może integrować dane pochodzące z dowolnego źródła, pod warunkiem że jest to źródło niezależne od już wykorzystywanych, a wykryte cechy geometryczne (wraz z niepewnością) zapisane są zgodnie z przyjętą reprezentacją i wyrażone względem lokalnego układu robota. Wobec powyższego warunkiem koniecznym użycia mapy lokalnej (listy cech) utworzonej przez innego robota jest określenie pozycji tego robota względem robota aktualizującego swoją mapę. A y B X R F B x X R F A F y y X R F B x y X KRB K x X R F A F x X KR A X R R B A Rysunek 5.3. Relacje między układami współrzędnych robotów podczas aktualizacji mapy (opis w tekście) W opisywanym systemie dostępne są dwa sposoby określenia wzajemnego usytuowania dwóch robotów. Gdy jeden z robotów wyposażony jest w kamerę pokładową, a drugi ma znacznik (landmark), to robot z kamerą może określić pozycję robota ze znacznikiem w swoim układzie współrzędnych (rys. 5.3A). Jeżeli robot R A, który w chwili k obserwuje n f cech 206

opisanych przez ˆx RAF i i C RAF i (i = 1...n f, indeksy chwili pominięto dla uproszczenia zapisu), jest wyposażony w znacznik, a robot R B ma kamerę pokładową, to pozycję i-tej cechy wykrytej przez robota R A w układzie robota R B można wyznaczyć, używając operacji złożenia transformacji: ˆx RBF i = ˆx RAF i ˆx RBR A, (5.1) C RBF i = J 1 C RAF i J T 1 + J 2 C RBR A J T 2, (5.2) gdzie ˆx RBR A jest estymatą pozycji robota R A otrzymaną z systemu wizyjnego robota R B. Roboty, które nie są wyposażone w kamery pokładowe lub nie mają znaczników, są pozbawione możliwości bezpośredniego określenia wzajemnego usytuowania. Mogą jednak wymienić dane dotyczące mapy wektorowej, jeżeli są jednocześnie obserwowane przez tego samego agenta percepcyjnego. W takiej sytuacji pozycje obu robotów w danej chwili mogą być określone względem wspólnego układu odniesienia, którym jest układ współrzędnych kamery monitorującej (rys. 5.3B). W przypadku wykorzystania kamery monitorującej przekształcenie układu robota R A, który obserwuje określone cechy w otoczeniu, w układ robota R B, który aktualizuje mapę wektorową, wymaga użycia operacji inwersji transformacji układu współrzędnych: ˆx RBF i = (ˆx RAF i ˆx KRA ) ( ˆx KRB ), (5.3) C KFi = J 1 C RAF i J T 1 + J 2 C KRA J T 2, (5.4) C RBK = J C KRB J T, (5.5) C RBF i = J 1 C KFi J T 1 + J 2 C RBKJ T 2, (5.6) gdzie ˆx KRA i ˆx KRB są odpowiednio estymatami pozycji robotów A i B otrzymanymi od agenta kamery monitorującej. Niezależnie od sposobu przekształcenia układów współrzędnych współpracujących robotów, cechy geometryczne opisane w układzie robota R B przez ˆx RBF i i C RBF i, otrzymane z zależności (5.1) i (5.2) lub (5.3 5.6), mogą być zintegrowane z mapą globalną robota R B na etapie estymacji stanu systemu. Opis cech geometrycznych przekazany przez innego robota traktowany jest w taki sam sposób jak obserwacje dokonane skanerem robota tworzącego mapę. W niepewności przekazanych cech jest uwzględniona niepewność przekształcenia układu robota dokonującego obserwacji w układ robota tworzącego mapę. Wykorzystanie cech zaobserwowanych przez innego robota jest więc całkowicie przeźroczyste dla algorytmu EKF-SLAM. W związku z tym implementacja agenta tablicowego realizującego SLAM (AESlamAlg) pozostaje taka sama niezależnie od tego, czy robot aktualizuje mapę samodzielnie, czy korzysta z pomocy innego agenta robota. Za transformację cech (5.1 5.6) odpowiedzialny jest agent AEMapUpd, który steruje także przepływem informacji pomiędzy współpracującymi robotami, obsługując kolejki komunikatów. 5.4. Architektura agenta percepcyjnego Bazę sprzętową agentów percepcyjnych stanowią kamery monitorujące pomieszczenie będące obszarem działania robotów mobilnych wchodzących w skład opisywanego systemu. Zadaniem agentów percepcyjnych jest określanie pozycji agentów robotów. Dzięki dostępności zewnętrznych źródeł informacji o pozycji można korzystać z robotów mobilnych, które nie mają zaawansowanych (a więc kosztowych) sensorów, takich jak 207

skaner laserowy, umożliwiających realizację SLAM lub samolokalizację w oparciu o dopasowywanie map. Funkcja ta jest przydatna także w sytuacjach, gdy robot mogący dokonywać samolokalizacji chwilowo nie ma możliwości realizacji tego zadania samodzielnie. Jest tak wówczas, gdy np. liczba rozróżnialnych cech geometrycznych otoczenia w polu widzenia sensorów pokładowych robota jest niewystarczająca. Sposób pozycjonowania robotów za pomocą kamer monitorujących zamontowanych u sufitu pomieszczenia oraz aktywnych znaczników LED umieszczonych na robotach szczegółowo przedstawiono w podrozdziale 2.5. Zadaniem agenta percepcyjnego jest świadczenie usług agentom robotom. W związku z tym obsługuje on kolejkę zgłoszeń usługobiorców. Na podstawie analizy zawartości tej kolejki agent percepcyjny może podać robotowi zgłaszającemu potrzebę pozycjonowania prognozowany czas wykonania tego zadania. Możliwość lokalizacji robotów mobilnych przez agenta percepcyjnego zależy od warunków oświetlenia, od zakłóceń powodowanych przez obiekty poruszające się w polu widzenia kamery oraz od położenia robota-zleceniodawcy względem osi optycznej kamery danego agenta [37] (por. rys. 2.49). Zgłaszając żądanie pozycjonowania, agent robot przesyła do agenta percepcyjnego swoją aktualną pozycję w globalnym układzie współrzędnych (estymatę pozycji, którą robot w danej chwili dysponuje). Na podstawie tej informacji oraz podanego w podrozdziale 2.5 modelu niepewności operacji pozycjonowania, który określa niepewność w zależności od położenia robota względem osi kamery monitorującej, agent percepcyjny prognozuje niepewność pozycji robota po pozycjonowaniu. W ten sposób zainteresowany agent robot może otrzymać informacje o spodziewanym czasie wykonania zadania i spodziewanej niepewności wyznaczonej pozycji przed rozpoczęciem operacji pozycjonowania, która wymaga zatrzymania robota na czas związany z akwizycją pary obrazów stanowiących podstawę określenia pozycji. Dane konfiguracyjne agenta percepcyjnego obejmują położenie i orientację związanej z nim kamery względem układu współrzędnych pomieszczenia (globalnego). Agenci percepcyjni nie są układami wielosensorycznymi, a przetwarzanie obrazu i wyznaczanie pozycji odbywa się w sposób sekwencyjny. Mimo to zdecydowano się nadać agentom percepcyjnym strukturę wewnętrzną zbliżoną do struktury agentów robotów. Tablicowy dostęp do danych ułatwia niezależne wykonywanie procesów przetwarzania obrazu i wyznaczania pozycji oraz obsługi protokołu negocjacji z agentami robotami. Obsługa tego protokołu obejmuje predykcję parametrów operacji pozycjonowania, którymi są niepewność i czas wykonania. Wokół tablicy przechowującej wszystkie dane niezbędne do działania agenta percepcyjnego, podobnie jak w przypadku architektury agenta robota, zgromadzono agentów urządzeń oraz agentów ekspertów (rys. 5.4). Ze względu na prostą strukturę sprzętową agenta percepcyjnego zestaw jego tablicowych agentów urządzeń jest ograniczony do następujących agentów: ADOvrCam agent kamery monitorującej. Działanie tego agenta jest inicjowane przez obecność nieobsłużonego zgłoszenia w kolejce. Dokonuje on akwizycji pary obrazów oraz wstępnie je przetwarza w celu uzyskania obrazu różnicowego, na podstawie którego następuje pozycjonowanie. Agent ten komunikuje się z obsługiwanym robotem (za pośrednictwem kolejki komunikatów InMsg i OutMsg) w celu synchronizacji operacji akwizycji obrazu z zapaleniem i zgaszeniem znaczników robota. Odnotowuje on także stan systemu wizyjnego (sprawny/niesprawny). 208

ADComm BLACKBOARD OutMsg AEReport InMsg KOLEJKA ZADAÑ STATUS POZYCJA ROBOTA NIEPEWNOŒÆ CZAS AENegProt ADOvrCam OBRAZ RÓ NICOWY AEOvrLoc Rysunek 5.4. Architektura agenta percepcyjnego ADComm agent komunikacji. Obsługuje fizyczny kanał komunikacyjny, którym w przypadku stacjonarnych sensorów jest przewodowy Ethernet. Interfejs komunikacyjny obsługuje taki sam zestaw komunikatów jak ADComm agentów robotów. Zestaw tablicowych agentów przetwarzających jest następujący: AEOvrLoc agent pozycjonowania na podstawie obrazu z kamery monitorującej. Jego działanie jest inicjowane przez pojawienie się na tablicy nowego obrazu z kamery. Odnajduje on obrazy znaczników LED i wyznacza położenie oraz orientację obserwowanego robota. AENegProt agent implementujący protokół negocjacji z obsługiwanymi robotami. Agent ten obsługuje kolejkę zgłoszeń, a także wyznacza spodziewaną niepewność pozycji robota żądającego wykonania pozycjonowania oraz spodziewany czas, w którym informacja o pozycji będzie dostępna. Po wykonaniu pozycjonowania estymuje niepewność pozycji obserwowanego robota. Działanie rozpoczyna po otrzymaniu komunikatu (w kolejce komunikatów przychodzących InMsg) od innego agenta lub gdy na tablicy dostępna jest wyznaczona pozycja obserwowanego robota. AEReport agent sprawozdawca. Działanie podejmuje co ustalony kwant czasu po wystąpieniu awarii systemu wizyjnego określonej odpowiednimi wartościami danych statusowych lub na żądanie operatora. Dodatkową zaletą przyjętej architektury jest jej modułowość, charakterystyczna dla podejścia agentowego. W przypadku agenta kamery monitorującej umożliwia to wymianę komponentu sprzętowego (np. frame-grabbera) bez naruszania integralności pozostałych modułów-agentów, dla których szczegóły programowych interfejsów ze sprzętem są nieistotne. 209

5.5. Struktura systemu komunikacji między agentami 5.5.1. Protokół negocjacji między agentami Chociaż w wewnętrznej architekturze agentów robotów i agentów percepcyjnych wykorzystuje się tablicowy sposób dostępu do danych, to komunikację pomiędzy agentami oparto na wymianie komunikatów z ustalonego zestawu [264]. Dzięki temu uniknięto problemów z dostępem do danych, które mogą wystąpić w systemie opartym na pojedynczej (centralnej) tablicy, jeżeli moduły przetwarzające tego systemu działają na fizycznie rozdzielonych zasobach obliczeniowych (roboty i komputery połączone siecią lokalną) [257]. Logiczna struktura komunikacji między agentami wzorowana jest na protokole kontraktacyjnym (ang. Contract Net Protocol CNP) [291]. Jest to dobrze znany z literatury protokół alokacji zadań, stanowiący pewną metaforę negocjacji handlowych, często stosowany w systemach rozproszonych (ang. distributed problem solving systems). W opisywanym systemie agenci roboty używają protokołu opartego na koncepcji CNP w celu wyboru najlepszego zewnętrznego źródła danych wykorzystywanych w nawigacji. Agent mający wykonać określone zadanie ogłasza istnienie tego zadania innym agentom, wysyłając do nich komunikat z żądaniem usługi (ang. request message). Ponieważ wszyscy agenci potrafią rozpoznawać ten sam zestaw komunikatów (operują tym samym językiem), to potencjalni dostawcy usług rozumieją treść żądania. Agenci zdolni do uczestnictwa w negocjacjach, zwani oferentami (ang. bidders), oceniają swoje możliwości w zakresie realizacji danego zadania (np. dostarczenia informacji o pozycji robota) i przesyłają oferty (ang. bids) do agenta, który wysłał żądanie. Oferta jest potwierdzeniem możliwości wykonania usługi i zawiera jej prognozowane parametry (czas wykonania usługi, estymowaną niepewność dostarczanych danych). Parametry te są metaforą proponowanego kosztu usługi. Agent, który zainicjował proces negocjacji, staje się jego zarządcą (ang. manager), zbierającym i oceniającym oferty wszystkich potencjalnych dostawców. Zarządca ocenia oferty otrzymane od poszczególnych uczestników negocjacji i przyznaje kontrakt na wykonanie zadania temu agentowi, którego oferta ma najkorzystniejsze parametry (najniższy koszt). Wybrany oferent otrzymuje od zarządcy komunikat potwierdzający kontrakt. Następuje faza wykonania usługi, w której agenci związani kontraktem przesyłają między sobą odpowiednie dane. Ponieważ w rozważanym systemie fizycznych urządzeń faza realizacji kontraktu może obejmować także aktywność inną niż komunikacja (np. obserwacja innego agenta), wprowadzono komunikat M_END_CON jawnie kończący kontrakt. Po jego odebraniu agent powraca do swoich poprzednich zadań. Agenci percepcyjni mogą być tylko oferentami, natomiast agenci roboty nie mają a priori przypisanej roli zarządcy lub oferenta i mogą przyjmować jedną z tych ról w zależności od zaistniałych okoliczności. Tabela 5.1. Komunikaty i parametry zadania lokalizacji robota przez agentów percepcyjnych Znaczenie Komunikat Nadawca Odbiorca Dane i parametry Żądanie M_REQ_LOC agent agent ˆx R(RA) bieżąca estymata pozycji robota pozycjonowania robot percepcyjny C R(RA) niepewność bieżącej pozycji robota Oferta M_BID_LOC agent agent C R(PA) spodziewana niepewność pozycji robota pozycjonowania percepcyjny robot Tloc spodziewany czas wykonania zadania Potwierdzenie M_ACK_CON agent agent kontraktu robot percepcyjny Pozycja i jej M_DAT_LOC agent agent ˆx R(PA) nowa estymata pozycji robota niepewność percepcyjny robot C R(PA) niepewność nowej pozycji robota 210

Jako przykład zastosowania opisanego mechanizmu negocjacji posłuży zadanie pozycjonowania robota przez agentów percepcyjnych. Zestawienie wykorzystywanych w tym zadaniu komunikatów wraz z ich parametrami zawiera tabela 5.1. Zleceniodawcą jest agent robot, który żąda od agentów percepcyjnych informacji o swoim aktualnym położeniu i orientacji i w tym celu przesyła komunikatm_req_loc. Agenci, którzy mogą wykonać zadanie pozycjonowania robota (znajduje się on w polu widzenia ich kamer), odsyłają swoje oferty M_BID_LOC, podając wartości parametrów stanowiących dla zleceniodawcy kryterium wyboru najkorzystniejszej z nich. Parametrami tymi są prognozowany czas pozycjonowania T loc (który zależy od liczby żądań oczekujących w kolejce por. rys. 5.4) oraz niepewność pozycji robota C R (PA) prognozowana na podstawie modelu niepewności operacji pozycjonowania z użyciem kamery monitorującej. Zarządca oczekuje przez pewien z góry określony czas T bid (zależny od liczby agentów w systemie [142]), by skompletować oferty od potencjalnych dostawców. Zbieranie ofert kończy się wraz z upływem czasu T bid lub stwierdzeniem, że nadeszły oferty od wszystkich potencjalnych dostawców zarejestrowanych w systemie. Otrzymane oferty są analizowane i odrzucane są te, w których czas T loc przekracza założoną wartość. Spośród pozostałych wybierana jest ta oferta, która charakteryzuje się najmniejszą prognozowaną niepewnością położenia. Skalarną miarą niepewności położenia jest pole elipsy niepewności wyznaczonej na podstawie macierzy C R (PA) za pomocą wzorów (2.22). Po dokonaniu wyboru zleceniodawca odsyła do zwycięskiego agenta potwierdzenie zamówienia M_ACK_LOC. Następnie agenci związani kontraktem wymieniają komunikaty zawierające rzeczywiste dane o pozycji robota. Tabela 5.2. Komunikaty i parametry zadania współpracy podczas tworzenia mapy Znaczenie Komunikat Nadawca Odbiorca Dane i parametry Żądanie M_REQ_MAP agent agent ˆx R bieżąca estymata pozycji robota mapy lokalnej robot robot r map promień obszaru zainteresowania robota Oferta mapy M_BID_MAP agent agent ˆx R bieżąca estymata pozycji robota lokalnej robot robot Tmap spodziewany czas wykonania zadania Potwierdzenie M_ACK_CON agent agent kontraktu robot robot Mapa lokalna M_DAT_MAP agent agent L RF1,... L RFnf lista cech (mapa lokalna) robot robot Innym przykładem użycia protokołu kontraktacyjnego do wymiany danych związanych z nawigacją robota jest aktualizacja mapy globalnej na podstawie danych przesłanych przez innego robota, wyposażonego w skaner laserowy, przy założeniu, że pozycja tego robota może być ustalona za pomocą kamery pokładowej. W tabeli 5.2 zestawiono komunikaty wykorzystywane w tym zadaniu. Zleceniodawcą jest agent robot, który od innych agentów robotów obserwujących otoczenie za pomocą skanera laserowego żąda mapy lokalnej utworzonej na podstawie bieżącego skanu. W tym przypadku oferty, które otrzymał robot będący zarządcą protokołu, są oceniane na podstawie relacji między jego pozycją a pozycją potencjalnego dostawcy. Relacja ta (odległość i kąt) determinuje niepewność, z jaką robot będzie mógł określić pozycję danego dostawcy. 211

5.5.2. Mechanizm komunikacji między agentami Opisana w poprzednim podrozdziale logiczna struktura negocjacji prowadzących do współpracy agentów determinuje sposób komunikacji między nimi oraz to, jakie informacje należy wymieniać i kiedy trzeba je przesyłać. Jednakże praktyczne rozwiązanie problemu komunikacji między robotami mobilnymi uzależnione jest w znacznym stopniu od użytego sprzętu. Gdy muszą być spełnione określone dla danej aplikacji ograniczenia (np. bardzo mała przepustowość kanału komunikacyjnego, energooszczędność), to używa się zazwyczaj protokołów komunikacyjnych specjalnie opracowanych na potrzeby danego systemu łączności [250]. Jeżeli natomiast warunki techniczne części sprzętowej systemu wielorobotowego pozwalają na użycie standardowych środków łączności bezprzewodowej, mogą być wykorzystane standardowe protokoły komunikacyjne używane w sieciach komputerowych [115]. Do tej kategorii zalicza się opisywany system, w którym wykorzystano mieszaną, częściowo bezprzewodową konfigurację sieci Ethernet, zgodną ze standardami IEEE 802.3 oraz IEEE 802.11b. Jako platformę systemową tworzonego oprogramowania wybrano Linux. Tabela 5.3. Typy usług zefiniowane w systemie robotów mobilnych i sensorów stacjonarnych Typ usługi Znaczenie ovr_loc_passive agent może być pozycjonowany za pomocą kamery monitorującej ovr_loc_active agent może wykonać pozycjonowanie za pomocą kamery monitorującej cam_loc_passive agent może być pozycjonowany za pomocą kamery pokładowej (umieszczono na nim landmark) cam_loc_active agent może wykonać pozycjonowanie za pomocą kamery pokładowej map_build_glb agent może tworzyć mapę globalną (wektorową) map_build_scn agent może dostarczyć mapę lokalną na podstawie skanu Zdecydowano się na wykorzystanie protokołu UDP (ang. User Datagram Protocol), który uznano za bardziej adekwatny do systemu komunikacji opartego na krótkich komunikatach niż popularniejszy TCP/IP [115]. Aby kontrola komunikacji była łatwa, scentralizowano zarządzanie systemem. Funkcję modułu zarządzającego pełni agent operator uruchomiony w jednym z komputerów stacjonarnych. Poszczególni agenci mogą się zgłaszać do uruchomionego systemu w sposób dynamiczny. Agent chcący dołączyć do systemu musi znać adres sieciowy agenta operatora. Adres ten pobierany jest z pliku konfiguracyjnego agenta. W celu dołączenia do systemu (logowanie) nowy agent wysyła na adres operatora żądanie rejestracji, podając swoją nazwę. Agent operator odpowiada na zgłoszenie nowego agenta, wysyłając mu jego unikatowy identyfikator Id(Agent) oraz numer portu, za pomocą którego będzie się odbywała dalsza komunikacja z tym agentem. Agent odpowiada komunikatem potwierdzającym zalogowanie do systemu. Operator uaktualnia bazę danych agentów (ang. Agent Data Base ADB) zawierającą informacje o poszczególnych agentach zarejestrowanych w danej chwili w systemie: nazwę agenta, identyfikator, adres IP, numer portu komuniacyjnego oraz listę usług, które dany agent może wykonać. Typy usług zdefiniowanych w systemie, a dotyczących wymiany informacji wspomagających nawigację robotów, przedstawiono w tabeli 5.3. Następnie agent operator rozsyła do wszystkich agentów kopie ADB. Aktywni w danej chwili agenci odpowiadają potwierdzeniem aktualizacji swoich kopii baz danych. Ponieważ zawartość bazy ADB jest rozsyłana do wszystkich agentów, znają oni możliwości percepcyjne poszczególnych członków zespołu. Pozwala to podczas negocjacji adresować zapytania tylko do określonej grupy 212

agentów i uniknąć rozgłaszania (ang. broadcasting), które dodatkowo obciąża sieć komputerową [79]. Agent operator, wysyłając komunikat M_ALIVE_ASK, sprawdza co pewien określony czas, czy pozostali agenci zalogowani w systemie są wciąż aktywni. Aktywny agent odpowiada komunikatem M_ALIVE_RESP. W przypadku braku odpowiedzi po upływie określonego czasu powyższe zapytanie jest powtarzane, a jeżeli po określonej liczbie prób łączność nie będzie ustanowiona, to dany agent będzie wyrejestrowywany z systemu. Agent operator aktualizuje wówczas ADB i rozsyła do pozostałych agentów. Komunikacja pomiędzy zarejestrowanymi w systemie agentami odbywa się bez pośrednictwa agenta operatora. Agent, który chce wysłać komunikat do innego agenta działającego w systemie, uzyskuje jego adres sieciowy z posiadanej kopii ADB. Dzięki temu możliwe jest bezpośrednie wysłanie komunikatu. Ponieważ protokół UDP nie obsługuje potwierdzenia dotarcia komunikatu i wznowienia łączności po jej utracie, to do realizacji tych zadań zastosowano prosty mechanizm oparty na rozwiązaniach wykorzystanych w protokole TFTP (ang. Trivial File Transfer Protocol) [114]. 5.6. Badania eksperymentalne 5.6.1. Pozycjonowanie z użyciem sensorów wizyjnych W celu sprawdzenia przydatności metod pozycjonowania z użyciem kamer monitorujących i sztucznych znaczników w systemie wieloagentowym oraz oceny skuteczności mechanizmu negocjacji z wykorzystaniem CNP wykonano szereg eksperymentów w pomieszczeniu Laboratorium Robotów Mobilnych IAiII wyposażonym w dwie kamery monitorujące i kilka sztucznych znaczników. W eksperymentach wykorzystano dwa roboty typu Labmate, z których jeden był wyposażony w kamerę pokładową (robot R B ), a drugi miał zamocowany do korpusu znacznik (robot R A, por. rys. 5.1). W pierwszym eksperymencie robot odtwarzał wcześniej zaplanowaną ścieżkę (rys. 5.5A) ze średnią prędkością około 0,6 m s, korzystając jedynie z odometrii i danych dostarczanych przez agentów percepcyjnych. Na rysunku 5.5C porównano rezultaty uzyskane, gdy robot wybierał usługi jednego z dwóch dostępnych agentów percepcyjnych na zasadzie pierwszego zgłoszenia, nie analizując ofert, oraz gdy negocjował wykonanie usługi pozycjonowania za pomocą protokołu CNP. Jako miarę jakości pozycjonowania wykorzystano pole powierzchni elipsy niepewności. Gdy robot nie używał protokołu CNP, lecz korzystał z informacji dostarczonych przez pierwszego dostępnego agenta percepcyjnego, liczba zapytań o pozycje była znacznie większa niż w drugim wypadku. Podczas eksperymentu agent robot w wielu punktach wielokrotnie powtarzał zapytania. Jedynie część prób lokalizacji zakończyła się sukcesem, tzn. niepewność położenia i orientacji zmalała do zakładanego poziomu. W niektórych punktach pole elipsy osiągnęło wielkość 700 cm 2. Było to spowodowane wybieraniem przypadkowych ofert pozycjonowania, mających małe szanse realizacji, np. gdy robot znajdował się na skraju pola widzenia kamery (rys. 5.5B), gdzie niepewność położenia znacznie wzrasta (por. rys. 2.49). W drugim przypadku agent robot negocjował usługi pozycjonowania za pomocą protokołu CNP i wybierał jedynie oferty mające szansę realizacji (mała niepewność przewidywana na podstawie pozycji robota). Drugi eksperyment pokazuje, w jaki sposób agent robot, który nie może w danym momencie skorzystać z usług agentów percepcyjnych, stosuje protokół CNP, by wykorzystać do 213

pole widzenia aktywnej kamery 2 [cm ] 800 700 600 500 400 300 200 przypadkowy wybór negocjacje A B 100 C 0 0 2 4 6 8 10 12 14 [m] Rysunek 5.5. Porównanie wyników pozycjonowania za pomocą kamer monitorujących (opis w tekście) pozycjonowania kamerę pokładową innego agenta robota. Na rysunku 5.6A przedstawiono sytuację, w której robot R A znajdował się w narożniku pomieszczenia, prawie poza polem widzenia agentów percepcyjnych, natomiast robot R B znajdował się w centrum pola widzenia jednej z kamer monitorujących. Gdy robot R A, mający landmark przymocowany do obudowy, potrzebował dokładnej informacji o swojej pozycji, wysłał zapytanie do wszystkich agentów w systemie, którzy zadeklarowali w bazie ADB możliwość wykonywania usługi typu loc_active, czyli pozycjonowania agenta robota z zewnątrz. Zapytanie zawierało parametry [x 0 y 0 θ 0 ] T bieżącej pozycji robota. Odpowiedział na nie jeden z agentów percepcyjnych oraz robot R B, który był wyposażony w kamerę pokładową i mógł wykonać zadanie pozycjonowania innego robota z landmarkiem (cam_loc_active). W ofercie agenta percepcyjnego był zdefiniowany spodziewany czas potrzebny na wykonanie pozycjonowania T posp A1 oraz spodziewana niepewność C AP A1. W tym przypadku niepewność wyznaczona na podstawie modelu zaproponowanego w podrozdziale 2.5.3 była duża ze względu na bardzo niekorzystne położenie robota R A względem kamery monitorującej (rys. 5.6B). Drugi z agentów percepcyjnych nie wysłał oferty, gdyż robot R A znajdował się całkowicie poza polem widzenia jego kamery. Oferta robota R B także zawierała spodziewaną niepewność pozycji robota z landmarkiem, jednak sposób jej wyznaczenia jest bardziej złożony. W przeciwieństwie do agentów percepcyjnych agent robot nie zawsze zna dokładnie swoją pozycję w globalnym układzie współrzędnych. Z tego względu oprócz przewidywanej niepewności określenia położenia i orientacji landmarku na robocie R A agent robot z kamerą pokładową musi znać estymatę swojej aktualnej pozycji. Robot dokonał predykcji niepewności pozycji landmarku względem własnego lokalnego układu współrzędnych C ARA za pomocą zależności wyprowadzonych w podrozdziale 2.5.4. By dokładnie określić swoją pozycję w układzie globalnym, robot R B mógł zwrócić się z zapytaniem do agentów percepcyjnych lub wykorzystać kamerę pokładową do lokalizacji innego landmarku sztywno związanego z otoczeniem. Spodziewana niepewność położenia określonego na podstawie obserwacji landmarku umieszczonego na ścianie była duża (rys. 5.6C) ze względu na znaczny kąt, pod jakim był on obserwowany. Znacznie mniejszą niepewność położenia oferował agent percepcyjny związany z kamerą monitorującą, gdyż robot R B znajdował się w centrum jego pola widzenia, gdzie niepewność jest najmniejsza. W konsekwencji robot R B zaakceptował ofertę agenta percepcyjnego, a następnie obliczył ostatecznie spodziewaną niepewność pozycji robota R A, biorąc pod uwagę oba składniki niepewności: C ARA oraz C BP A2 i wysłał swoją ofertę (rys. 5.6D). 214

A ROBOT_A C E B ROBOT_B D F Rysunek 5.6. Kooperatywne pozycjonowanie za pomocą metod wizyjnych (opis w tekście) Robot R A odebrał obie oferty, porównał spodziewaną niepewność położenia, wybrał ofertę robota R B, a następnie wysłał komunikat potwierdzający kontrakt. Nastąpiła faza realizacji kontraktu, w której robot R B dokonał pozycjonowania z pomocą agenta percepcyjnego i odebrał od niego rzeczywiste dane dotyczące swojego położenia i orientacji wraz z macierzą kowariancji C BP A2 wyznaczoną z uwzględnieniem rzeczywistych współrzędnych odczytanych z obrazu. W tym samym czasie robot R B określił pozycję landmarku na robocie R A : dokonał akwizycji obrazu z kamery pokładowej i przetworzył go, po czym wyznaczył rzeczywiste wartości współrzędnych landmarku w swoim lokalnym układzie współrzędnych (rys. 5.6E i 5.6F) i na ich podstawie obliczył niepewność C ARA. Ostatnim etapem wykonania kontraktu było przekształcenie wektora x ARA i niepewności C ARA we współrzędne globalne z uwzględnieniem niepewności C BP A2, z którą była wyznaczona pozycja x BP A2 w układzie globalnym. Tak przygotowane dane zostały wysłane do robota R A jako nowa estymata jego pozycji x Rn w układzie globalnym i niepewność tej estymaty C Rn. Następnie zarządca protokołu rozwiązał kontrakt, wysyłając do robota R B odpowiedni komunikat. 5.6.2. Współdziałanie podczas tworzenia mapy otoczenia W zaprezentowanej w niniejszym rozdziale wersji architektury systemu robotów mobilnych i sensorów stacjonarnych roboty tworzą globalne mapy otoczenia, wykorzystując system LS/EKF-SLAM. W przedstawionej we wcześniejszych pracach [63, 282] wersji tej architektury roboty nie miały takiej możliwości, tworzyły mapy, dokonując samolokalizacji w oparciu o sztuczne znaczniki [38] lub wykorzystując dane o pozycji dostarczone przez kamery monitorujące. Opisany w podrozdziale 4.4 system LS/EKF-SLAM zaimplementowano jednak tylko na robocie TRC Labmate, wyposażonym w skaner laserowy LMS 200. Uznano, że zainstalowany na drugim z robotów Labmate skaner klasy AMCW (por. p. 2.4.2), charakteryzujący się niewielkim zasięgiem i bardzo małą szybkością akwizycji pomiarów, nie stanowi odpowiedniego źródła danych dla LS/EKF-SLAM. Tak więc tylko jeden z robotów wchodzących w skład stanowiska doświadczalnego w Laboratorium Robotów Mobilnych może jednocześnie dokonywać samolokalizacji i tworzyć mapę wektorową. Uniemożliwia 215

to pełną eksperymentalną weryfikację procedury pozwalającej na wykorzystanie danych ze skanera laserowego innego agenta robota. Można było jednak sprawdzić z użyciem jednego robota poprawność działania LS/EKF-SLAM podczas aktualizacji mapy na podstawie danych z innego robota, który jest jednocześnie obserwowany przez tę samą kamerę monitorującą. Podczas tego eksperymentu TRC Labmate wystąpił kolejno w roli obu współpracujących ze sobą robotów. A B Rysunek 5.7. Indywidualne mapy utworzone podczas eksperymentu Wykonując eksperyment założono, że mapę globalną tworzy robot R A, a robot R B dostarcza mapy lokalne opisujące te obszary, które znajdują się poza polem widzenia skanera robota R A lub na granicy tego pola. Wyboru punktów ścieżki, w których R A inicjował poszukiwanie zewnętrznego dostawcy mapy lokalnej, dokonano ręcznie przed rozpoczęciem eksperymentu, ponieważ w oprogramowaniu robota nie zaimplementowano strategii kooperatywnej eksploracji otoczenia. Problem kooperatywnej eksploracji 1 [249, 267, 306], jakkolwiek istotny dla uzyskania pełnej autonomii zespołu robotów mobilnych, wykracza poza zakres tematyczny niniejszej pracy. Podczas opisywanego eksperymentu najpierw nastąpił przejazd odwzorowujący działanie robota R B. Mapa wektorowa utworzona przez system LS/EKF-SLAM podczas tego przejazdu jest przedstawiona na rys. 5.7B w postaci trójwymiarowej. Podczas tego przejazdu robot zatrzymywał się w punktach, w których była uaktywniana procedura EKF-SLAM (pozycje zaznaczone na rys. 5.8 kwadratami o przerywanej linii) i nawiązywał kontakt z agentem percepcyjnym kamery monitorującej, który wyznaczał jego pozycję (rys. 5.8A). Dane z eksperymentu odwzorowującego działanie robota R B zostały zapisane w plikach. Dane te obejmowały mapy lokalne utworzone w poszczególnych punktach wzdłuż ścieżki oraz pozycje robota w tych punktach otrzymane z kamery monitorującej. Opracowano program symulujący działanie interfejsu komunikacyjnego agenta robota, który miał dostęp do plików z danymi robota R B. W ten sposób fizyczny agent robot R A mógł otrzymywać rzeczywiste dane od wirtualnego agenta robota R B. Następnie robot Labmate wykonał przejazd wzdłuż innej ścieżki (zaznaczona kwadratami o ciągłej linii na rys. 5.8), odwzorowując działanie robota R A. W pierwszym wybranym punkcie ścieżki robot R A zatrzymał się i wysyłał żądanie M_REQ_MAP do wszystkich agentów w systemie, którzy zadeklarowali w bazie ADB zdolność do realizacji usługi map_build_scn. Zapytanie zawierało parametry bieżącej pozycji robota R A oraz maksymalną odległość d map, z której robot R A może określić pozycję innego robota. Ponieważ R A jest wyposażony w kamerę pokładową, d map jest maksymalną odległością identyfikacji sztucznego znacznika. W rozważanym przypadku na żądanie odpowiedział jedynie pro- 1 Może być on rozpatrywany jako szczególny przypadek zagadnienia alokacji zadań. 216

A B C Rysunek 5.8. Wynik współdziałania podczas tworzenia mapy otoczenia gramowy agent symulujący robota R B. W jego ofercie były określone: spodziewany czas wykonania usługi T map (czas skanowania i ekstrakcji cech) i bieżąca estymata pozycji ˆx RB. Po upływie czasu T bid robot R A zaakceptował ofertę jedynego dostawcy R B, a następnie wysłał komunikat potwierdzający kontrakt. W odpowiedzi agent R B wysłał do R A komunikat M_DAT_MAP zawierający mapę lokalną odczytaną z pliku. Mapa ta powstała na podstawie rzeczywistego skanu wykonanego w punkcie odpowiadającym bieżącej pozycji R B. Ponieważ agent R B (którego identyfikator robot R A otrzymał w komunikacie) nie zadeklarował w ADB usługicam_loc_passive, robot R A nie mógł określić jego pozycji za pomocą kamery pokładowej. W tej sytuacji wysłał żądanie pozycjonowania M_REQ_LOC do agentów percepcyjnych kamer monitorujących. Agent percepcyjny, w którego polu widzenia znajdował się robot R A, odpowiedział ofertą, która jako jedyna nadesłana została przyjęta. Nastąpiła faza realizacji kontraktu, R A odebrał rzeczywiste dane dotyczące swojej pozycji oraz jej niepewności. Następnie wysłał do agenta R B żądanie podania jego dokładnej pozycjim_req_pos. W rzeczywistej sytuacji robot R B także wysłałby żądanie pozycjonowania do agentów percepcyjnych, jednak symulujący go podczas opisywanego eksperymentu agent programowy odczytał zapisaną w pliku pozycję (otrzymaną uprzednio z rzeczywistej kamery monitorującej) i wysłał ją do R A w postaci komunikatum_dat_pos. Robot R A, będący w tym wypadku zarządcą protokołu, odpowiedział komunikatem M_ACK_END, kończąc tym samym kontrakt. Dysponujący już wszystkimi potrzebnymi danymi robot R A dokonał przekształcenia układu współrzędnych mapy lokalnej otrzymanej od R B w swój układ współrzędnych, korzystając z zależności (5.3 5.6). Pozwoliło to zainicjować działanie algorytmu EKF-SLAM i zintegrować dane otrzymane od robota R B w globalnej mapie wektorowej robota R A. Opisaną powyżej procedurę powtórzono we wszystkich uprzednio wybranych punktach ścieżki robota R A. Mapę globalną utworzoną przez tego robota z uwzględnieniem danych otrzymanych z zewnątrz przedstawiono w postaci widoku trójwymiarowego na rys. 5.8B i C. Dla porównania na rys. 5.7A zaprezentowano mapę powstałą wyłącznie na podstawie danych zgromadzonych przez robota R A. Wymiana danych między robotami pozwoliła zbudować pełniejszy model otoczenia. Wykorzystanie informacji o pozycjach robotów dostarczonej przez agenta percepcyjnego umożliwiło skuteczną integrację cech uzyskanych z zewnątrz na mapie globalnej, która nie jest prostą sumą cech geometrycznych zgromadzonych na mapach obu robotów. 217

5.7. Wnioski W niniejszym rozdziale zaproponowano architekturę rozproszonego systemu robotów i sensorów stacjonarnych, w której wykorzystano przedstawione uprzednio moduły funkcjonalne oraz nowe techniki przetwarzania i gromadzenia informacji o otoczeniu robota. Podstawowe rozwiązania strukturalne dotyczą architektur oprogramowania agentów związanych z poszczególnymi komponentami systemu rozproszonego (robotami mobilnymi i kamerami monitorującymi) oraz systemu wymiany informacji między agentami, opartego na protokole kontraktacyjnym. Najistotniejsze cechy agentowej architektury tablicowej robota mobilnego to jednoczesne użycie różnych rodzajów reprezentacji otoczenia (rastrowej i wektorowej) oraz różnych koncepcji opisu niepewności (probabilistycznego i rozmytego). W porównaniu z poprzednimi pracami [62, 63, 282] architektura robota mobilnego została zmodyfikowana, a jej rdzeniem stał się system jednoczesnej samolokalizacji i tworzenia wektorowej mapy otoczenia, zgodnie z koncepcją LS/EKF-SLAM. Struktura tablicy i zestaw agentów zostały dostosowane do tego zadania. System nawigacji pojedynczego robota, w którym wykorzystuje się kilka różnych reprezentacji otoczenia, zaproponowano w pracy [102]. W tym rozwiązaniu modele o wyższym poziomie abstrakcji (mapa wektorowa i topologiczna) powstają przez wyodrębnienie odpowiednich informacji z mapy rastrowej, stanowiącej podstawową reprezentację informacji przestrzennej, która jako jedyna jest aktualizowana bezpośrednio przez sensory robota. W przeciwieństwie do architektury tablicowej w podejściu zaproponowanym w pracy [102] wprowadzono sztywną hierarchię map. Brak takiej hierarchi w architekturze MAB pozwala na przetwarzanie danych z poszczególnych rodzajów sensorów w taki sposób, jaki jest dla nich najkorzystniejszy, np. dane z sonarów, obarczone największą niepewnością co do przedmiotu pomiaru, służą jedynie do aktualizacji mapy rastrowej, która jest względnie odporna na tego rodzaju niepewność, natomiast dane ze skanera laserowego poddawane są bezpośredniemu przetworzeniu na mapę wektorową, co pozwala wykorzystać efektywne metody grupowania i segmentacji, dające lepsze wyniki niż pośrednie odtwarzanie odcinków z mapy rastrowej (por. p. 3.4.4). Architektura robota mobilnego jest częścią architektury całego systemu rozproszonego. Podstawowym zadaniem mechanizmu komunikacyjnego między agentami w tym systemie jest umożliwienie agentowi robotowi znalezienia alternatywnego źródła informacji wspierającej nawigację autonomiczną w sytuacji, gdy robot ten nie może wykonać powierzonego mu zadania z powodu niemożności uzyskania takiej informacji z sensorów pokładowych. Podobne zagadnienie współpracy podczas budowy modelu otoczenia zostało podjęte w pracy [65], gdzie przedstawiono metodę tworzenia mapy rastrowej przez grupę prostych, homogenicznych robotów, zilustrowaną wynikami symulacji. W przeciwieństwie do propozycji przedstawionej w niniejszym rozdziale rozwiązanie z pracy [65] ma charakter scentralizowany, nie pozwala na współistnienie różnych reprezentacji otoczenia, a wymiana informacji między robotami odbywa się na niższym poziomie abstrakcji danych. W opisanym w pracy [267] systemie wielorobotowym, którego zadaniem jest kooperatywna budowa modelu otoczenia i eksploracja, wykorzystuje się koncepcję ofert podobną do zaproponowanej w publikacjach [155, 282] oraz użytej w niniejszej pracy. Istotną różnicą jest jednak poziom abstrakcji przesyłanej informacji, którą stanowią nieprzetworzone skany, oraz użycie centralnej jednostki przetwarzającej, oceniającej oferty i zbierającej dane od poszczególnych robotów. Spośród opisywanych w literaturze architektur robotów mobilnych najbardziej zbliżonym rozwiązaniem pod względem sposobu reprezentacji i użycia danych sensorycznych do zapre- 218

zentowanej tutaj wersji architektury MAB wydaje się system opracowany w LAAS w Tuluzie [10]. Podobnie jak w architekturze tablicowej, wykorzystuje się w nim zestaw modułów funkcjonalnych o charakterze czarnych skrzynek oraz skojarzone z tymi modułami wyspecjalizowane reprezentacje danych sensorycznych, nazywane posterami [10]. Istotna różnica leży w przyjęciu innego sposobu organizacji współpracy modułów oraz innego poziomu abstrakcji reprezentowanej informacji. Opisane w artykule [10] moduły reprezentują poszczególne funkcje robota i współpracują ze sobą na zasadzie klient/serwer, w przeciwieństwie do agentów tablicowych, którzy komunikują się ze sobą jedynie poprzez wymianę danych w określonych strukturach tablicy [64]. Przypisane do modułów postery reprezentują informacje właściwe danemu zadaniu, a nie różne opisy otoczenia (mapy lokalne lub globalne) jak w architekturze MAB. W przeciwieństwie do kwestii wymiany danych przydatnych podczas budowy modelu otoczenia ogólna problematyka wymiany informacji w grupach robotów była studiowana przez wielu badaczy. W systemach wielorobotowych opartych wyłącznie na sterowaniu odruchowym (a więc pozbawionych komponentów planujących) wykorzystuje się kanały komunikacyjne do wyrażenia zależności między poszczególnymi zachowaniami robotów tworzących zespół [322]. Rozwiązanie to stanowi rozproszony odpowiednik architektury uogólniającej (ang. subsumption architecture) [57], a kanały wymiany sygnałów (komunikatów) stanowią w nim część sieci zachowań obejmującej cały zespół. Wymiana informacji dotyczy wykonywanych w danej chwili procedur-zachowań i ma charakter prostych sygnałów aktywujących określone zachowania innych agentów. W opartej także na paradygmacie behawioralnym architekturze ALLIANCE [232] minimalizuje się rolę wymiany informacji w ramach jawnej (intencjonalnej) komunikacji między robotami. Wyposażone w łącza radiowe identyczne roboty należące do zespołu rozsyłają informacje o swoim stanie, lecz nie oczekują na odpowiedź i nie nawiązują łączności w celu wymiany zgromadzonych danych. Rozsyłanie informacji spełnia tu funkcję komunikacji poprzez środowisko, gdyż proste roboty używane w eksperymentach opisanych w rozprawie [232] nie mogą wzajemnie obserwować swojego stanu za pomocą sensorów zewnętrznych. Podobnie minimalistyczne podejście do komunikacji między robotami przyjęto w pracy [118]. Rozważany jest tam jednak przypadek heterogenicznego zespołu robotów, a komunikaty mają konkretnego adresata. Jak zauważają autorzy [118], bardzo uproszczona komunikacja jest bezpośrednim rezultatem przyjętego minimalistycznego modelu współpracy robotów. Podejście to zostało przeniesione na zagadnienie kolektywnego przeszukiwania pomieszczeń przez zespół jednorodnych robotów, omówione w artykule [55]. W systemach wieloagentowych opisanych w przytoczonych powyżej pracach określone działania robotów są inicjowane przez komunikaty otrzymane od innych członków zespołu. W architekturze MAB komunikaty nadchodzące z zewnątrz modyfikują dane na tablicy poprzez agenta, który zarządza danym obszarem tablicy, np. AEPosUpd, a poszczególne moduły funkcjonalne pobierają dane z tablicy lub sensorów i nie mają dostępu do poziomu komunikacji międzyagentowej. W literaturze opisano także wiele architektur agentowych odnoszących się do systemów czysto programowych (ang. software agents) [11, 60, 244]. Architektury te są często opracowywane w celu umożliwienia integracji agentów programowych różnego rodzaju i pochodzenia (np. pochodzących od różnych dostawców oprogramowania). Z tego względu podstawę komunikacji międzyagentowej stanowią w nich dobrze zdefiniowane, standardowe języki komunikacji, takie jak KQML (ang. Knowledge Query and Manipulation Language) zorientowany na komunikaty język wymiany informacji, niezależny od języka opisu treści 219

komunikatów (ang. content language) i zakładanej ontologii [204]. Ogólność takiego podejścia do komunikacji, będąca zaletą w architekturach agentów programowych, wiąże się z dodatkowymi nakładami projektowymi (np. definicją ontologii) oraz rozbudową samych komunikatów. W systemie agentów sprzętowo-programowych, działających w czasie rzeczywistym i zaprojektowanych od podstaw jako komponenty jednego systemu, taka ogólność języka komunikacji nie jest potrzebna. Komunikacja między agentami odbywa się za pomocą komunikatów o prostej składni, możliwe też było użycie logicznej struktury protokołu kontraktacyjnego. Szczegółowy opis mechanizmów komunikacji opracowanych na potrzeby prezentowanego systemu wielorobotowego wykracza poza ramy niniejszej pracy, a zainteresowany czytelnik znajdzie go wraz z analizą złożoności komunikacyjnej, efektywności i skalowalności w pracach [155, 281, 282]. Szczegóły dotyczące implementacji biblioteki funkcji komunikacyjnych dołączanej do oprogramowania agentów przedstawiono natomiast w raporcie [264]. Rola wymiany informacji dotyczących środowiska w wieloagentowych zespołach robotów mobilnych była badana przez Arkina i Balcha [29]. Wykazali oni za pomocą symulacji, że możliwość nawiązania intencjonalnej komunikacji między agentami i wymiana informacji o stanie środowiska i/lub samych agentów w zależności od rodzaju zadania w różnym stopniu poprawia efektywność działania zespołu robotów. W szczególności stwierdzono, że jeżeli wykonywane zadanie zapewnia możliwość komunikacji poprzez środowisko (wzajemną obserwację), to intencjonalna wymiana komunikatów nie przynosi oczekiwanej poprawy efektywności działania. Podkreślono także fakt, że komunikacja intencjonalna jest formą aktywności agenta, podobnie jak akcje podejmowane dzięki efektorom robota. Komunikowanie się pociąga więc za sobą określone koszty (obliczeniowe, energetyczne, czasowe). Przed podjęciem akcji komunikacyjnej agent powinien rozważyć potencjalne zyski, jakie może mu przynieść taka akcja, i porównać je z kosztami podejmowanej akcji komunikacyjnej. Przedstawione w niniejszym rozdziale eksperymenty potwierdzają, że w rozproszonym systemie robotów i sensorów stacjonarnych wymiana informacji realizowana w formie jawnej komunikacji poprawia możliwości nawigacyjne robotów. Zaproponowana struktura komunikacji między agentami pozwala na ocenę użyteczności informacji uzyskiwanej od innych agentów oraz prognozowanie czasu jej otrzymania. Dzięki temu istnieje możliwość nawiązania komunikacji z tym dostawcą informacji, który oferuje najlepsze warunki jej uzyskania. Mechanizm wymiany informacji, obejmujący wysyłanie ofert, których jednym z parametrów jest prognozowana niepewność tej informacji, zapewnia możliwość stwierdzenia, czy dane udostępniane przez innych agentów są obarczone mniejszą niepewnością niż dane lokalne (z sensorów pokładowych) i czy poniesienie kosztów odwoływania się do zewnętrznych źródeł danych jest w danym przypadku uzasadnione.

Rozdział 6 Planowanie działań robota z uwzględnieniem niepewności percepcji 6.1. Wprowadzenie W poprzednim rozdziale zaprezentowano architekturę systemu robotów mobilnych współpracujących z sensorami stacjonarnymi. Działający w tym systemie robot mobilny, którego zestaw sensorów pokładowych nie umożliwia realizacji procedury SLAM, może wykorzystać do samolokalizacji kamerę pokładową obserwującą rozmieszczone w otoczeniu sztuczne znaczniki lub może odwołać się do agentów percepcyjnych związanych z kamerami monitorującymi. Protokół kontraktacyjny pozwala agentowi robotowi wybrać spośród dostępnych źródeł informacji to, które dostarczy dane o jego pozycji obarczone najmniejszą niepewnością położenia. Ponieważ samolokalizacja za pomocą kamery pokładowej i kamer monitorujących ma charakter dyskretnych akcji pozycjonujących, wykonywanych jedynie w określonych chwilach i w określonym punkcie ścieżki, którą porusza się robot, to system nawigacji robota musi zawierać określoną strategię pozycjonowania, która pozwala ustalić, kiedy robot powinien rozpocząć poszukiwanie informacji zmniejszających niepewność jego pozycji. W istniejącej implementacji konieczność podjęcia akcji pozycjonowania jest ustalana na podstawie obserwacji macierzy kowariancji pozycji robota. Pomiędzy kolejnymi akcjami pozycjonującymi pozycja robota określana jest tylko na podstawie odometrii, a niepewność pozycji rośnie (por. wzór 2.26). Gdy pole powierzchni elipsy niepewności wyznaczonej na podstawie macierzy C R osiągnie założony z góry próg C max, robot rozpoczyna realizację zadania pozycjonowania za pomocą zewnętrznej infrastruktury (znacznika lub kamery monitorującej). Wyniki niektórych eksperymentów wykazały jednak wady tej bardzo prostej strategii pozycjonowania [284]. Zaobserwowano, że w niektórych punktach ścieżki robota niepewność jego położenia znacznie przekracza ustalony próg C max. Jest to spowodowane lokalnym (w sensie przestrzennym) charakterem negocjacji za pomocą protokołu kontraktacyjnego. Zapewnia on użycie najlepszej z dostępnych w danym miejscu i czasie (czyli w danym punkcie ścieżki) estymaty pozycji robota, jednak wybór punktu, w którym procedura negocjacji 221

jest inicjowana, nie jest optymalny. Robot nie wykorzystuje informacji o rozmieszczeniu elementów zewnętrznej infrastruktury nawigacyjnej 1, a sposób wyboru punktu, w którym jest inicjowane pozycjonowanie, ma charakter strategii zachłannej robot zawsze wyczerpuje limit niepewności określony jako C max. W związku z tym zaobserwowano sytuacje, w których niepewność nie może być zredukowana przed przekroczeniem zadanego progu, ponieważ w danym punkcie ścieżki nie można podjąć żadnej skutecznej akcji pozycjonującej. Jeżeli jednak struktura systemu (liczba i rozmieszczenie kamer monitorujących oraz sztucznych znaczników w otoczeniu) jest stała, a robot ma dostęp do kompletnej informacji o tej strukturze oraz ma wyznaczoną wcześniej bezkolizyjną ścieżkę prowadzącą do wybranego celu, to może zaplanować optymalną strategię pozycjonowania wzdłuż tej ścieżki, zanim rozpocznie wykonywanie zadania. W literaturze opisano wiele algorytmów planowania ścieżki z uwzględnieniem niepewności pozycji robota. Większość z tych prac dotyczy jednak robotów wyposażonych w pokładowe sensory zapewniające ciągłą lub w przybliżeniu ciągłą obserwację otoczenia i możliwość samolokalizacji oraz dysponujących kompletnym, geometrycznym modelem otoczenia. Jednym z bardziej znanych podejść do planowania ścieżki robota z uwzględnieniem jego możliwości w zakresie pozycjonowania jest zaproponowana w pracy [300] metoda pola niepewności sensora (ang. Sensory Uncertainty Field SUF). Jej twórcy przyjęli założenie, że robot dysponuje dokładnym modelem otoczenia i jest wyposażony w idealny sensor odległości, który zapewnia w przybliżeniu ciągłą samolokalizację. W koncepcji SUF zakłada się wyznaczenie wartości określających niepewność pozycji w danej konfiguracji robota przez wielokrotne symulowanie pomiarów i samolokalizacji. Po wykonaniu symulacji dla wszystkich bezkolizyjnych konfiguracji budowana jest regularna siatka, zwana polem niepewności sensora. Metoda pola niepewności koncepcyjnie przypomina przedstawione w podrozdziale 2.5 mapy niepewności położenia, sporządzone dla zewnętrznych sensorów wspomagających pozycjonowanie robota. W przeciwieństwie do SUF mapy te uzyskano na podstawie wzorów opisujących zależność macierzy C R od konfiguracji robota względem danego sensora. Dzięki temu nie trzeba zakładać z góry określonej rozdzielczości mapy niepewności, gdyż miara niepewności może być obliczona dla dowolnej pozycji robota. W wyniku proponowanej w artykule [300] procedury planowania ruchu robota otrzymuje się ścieżkę, która pozwala zminimalizować oczekiwaną niepewność pozycji przez wybór tych konfiguracji bezkolizyjnych, w których cechy środowiska przydatne podczas pozycjonowania są najlepiej widoczne. Koncepcja SUF wykorzystywana była w wielu pracach, także w odniesieniu do sensorów wizyjnych [2]. W wielu pracach dotyczących planowania ruchu robota mobilnego z uwzględnieniem niepewności pozycji czynione są daleko idące uproszczenia w przyjętych modelach sensorów, np. idealny pomiar odległości lub landmarki zapewniające pozycjonowanie ze stałą niepewnością w danym obszarze [177]. Jedynie w kilku publikacjach dotyczących planowania [110, 176, 208, 211] wykorzystywany jest probabilistyczny model niepewności pozycji, zbliżony do zaproponowanego w podrozdziale 2.5. W artykule [110] zaproponowano model niepewności pozycji robota w postaci macierzy kowariancji. Macierz ta określa elipsę niepewności, wewnątrz której z danym prawdopodobieństwem znajduje się robot. Podczas planowania ścieżki bierze się pod uwagę możliwość pozycjonowania na podstawie odometrii i za pomocą skanera laserowego. Zakłada się znajomość modelu otoczenia. Wyznaczana ścieżka składa się z punktów, w których może być 1 Zgodnie z założeniami architektury systemu wieloagentowego liczba i właściwości jego komponentów mogą ulegać zmianom. 222

wykonana akcja pozycjonująca. Określane są punkty, w których robot może dokonać samolokalizacji, a następnie na ich podstawie wyznaczany jest graf. Graf ten przeszukuje się w celu ustalenia optymalnej ścieżki. Algorytm przeszukiwania zawiera funkcję kosztu, która jest sumą współczynników określających jakość pozycjonowania, oraz euklidesową odległość między wierzchołkami grafu. Podejście to zostało rozszerzone w artykule [176] na robota wyposażonego w kamerę pokładową i określającego swoją pozycję względem zdefiniowanych w otoczeniu landmarków. Probabilistyczny model niepewności dla sensorów wizyjnych przyjęto także w pracach [208, 211]. Wykorzystano pokładowy system stereowizyjny wykrywający naturalne cechy otoczenia, którymi są pionowe krawędzie. Niepewność pozycji robota wyznaczonej na podstawie obserwacji tych cech jest reprezentowana przez macierz kowariancji. Robot ma dostęp do geometrycznego modelu otoczenia, lecz planowanie odbywa się on line i dotyczy jedynie następnego ruchu określany jest kolejny punkt pozycjonowania, który zapewnia bezkolizyjny ruch robota z uwzględnieniem niepewności oraz najmniejszą długość ścieżki. Uzyskana ścieżka nie jest więc optymalna w sensie globalnym. W niniejszej pracy przyjęto nieco inne założenia i cele niż w przytoczonych wyżej publikacjach, co wynika głównie z zastosowania w systemie mobilnych robotów współpracujących z zewnętrzną aktywną infrastrukturą oraz z właściwości wizyjnych metod określania pozycji robota. Zachowanie robota obserwującego otoczenie za pomocą metod wizyjnych jest oprtunistyczne, tzn. robot poruszający się po zaplanowanej wcześniej ścieżce aktualizuje swoją pozycję jedynie wtedy, gdy ma ku temu okazję postrzega sztuczny znacznik (landmark) lub jest widziany przez kamerę monitorującą. Model otoczenia dostępny na tym etapie planowania jest ograniczony do rozmieszczenia (pozycji i orientacji) stałych elementów infrastruktury zewnętrznej kamer i znaczników. W modelu tym nie są uwzględnione wszystkie obiekty, dzięki czemu jego budowa i utrzymanie w stanie aktualnym nie jest zadaniem pracochłonnym. W związku z tym w wyniku procedury planowania nie otrzymuje się optymalnej ścieżki (zakłada się, że bezkolizyjna ścieżka od startu do celu jest znana a priori), lecz sekwencję akcji pozycjonujących, która pozwala minimalizować przyjęte kryterium. Kryterium tym jest czas zużyty przez robota na pozycjonowanie podczas ruchu od startu do celu. Czas tracony podczas akwizycji i analizy obrazu (obserwacja znaczników) oraz podczas komunikacji i negocjacji z agentami percepcyjnymi traktowany jest jako koszt pozycjonowania. Zadaniem zaplanowanej sekwencji akcji pozycjonujących nie jest minimalizacja niepewności pozycji robota, lecz utrzymanie niepewności poniżej założonego progu, a minimalizowany jest koszt pozycjonowania. Podejście to jest uzasadnione z praktycznego punktu widzenia: minimalizacja niepewności za wszelką cenę nie daje istotnych korzyści, do wykonania określonego zadania (np. przejazdu przez otwarte drzwi [211]) wymagana jest znajomość pozycji z określoną, dopuszczalną niepewnością, natomiast optymalizacja wykorzystania zewnętrznej infrastruktury, a szczególnie kamer monitorujących, których liczba jest ograniczona, ma bardzo istotne znaczenie w systemie z wieloma robotami [39, 282]. Nominalna ścieżka bezkolizyjna wyznaczana jest przed planowaniem akcji za pomocą odrębnego planera geometrycznego, który uwzględnia początkową i docelową konfigurację robota oraz znane przeszkody. Implementując prezentowaną tu metodę planowania akcji jako procedurę planowania bezkolizyjnej drogi, wybrano metodę diagramu Voronoia [178]. Metoda ta pozwala uzyskać bardzo bezpieczne drogi (tory ruchu) robota, przebiegające możliwie daleko od przeszkód. Etap planowania geometrycznego prowadzony jest dla robota traktowanego jako punkt i dla obiektów na mapie powiększonych o promień okręgu opisane- 223

go na robocie. Tak zaplanowana ścieżka jest wykonalna dla robota o układzie jezdnym typu różnicowego, który może zmienić orientację bez zmiany pozycji (obrócić się w miejscu). W otrzymanej sieci dróg (diagramie Voronoia) najkrótszą drogę od startu do celu znajduje się z użyciem algorytmu A. 6.2. Model grafowy przestrzeni akcji Proponowany sposób planowania akcji pozycjonujących jest oparty na klasycznym podejściu do znajdowania najkrótszych dróg w grafie [8, 295]. Należy zbudować graf reprezentujący wszystkie stany analizowanego układu i dopuszczalne przejścia między nimi, a następnie znaleźć w nim drogę (sekwencję stanów) minimalizującą przyjęte kryterium optymalności. A œcie ka robota 1 pole widzenia kamery monitoruj¹cej B 1 C 1 C 1,4 > C max 2 3 4 v i C i 2 T j C i,j T k C i,k akcje pozycjonuj¹ce landmark przestrzeñ akcji (graf) 3 vj C j T k C j,k vkc k 4 Rysunek 6.1. Koncepcja przestrzeni akcji jako grafu (opis w tekście) W rozpatrywanym przypadku stanami układu są akcje pozycjonujące podejmowane przez robota mobilnego. Koncepcja akcji pozycjonujących została przedstawiona na rys. 6.1A. Nominalna ścieżka robota P nom, zaplanowana tak, by uniknąć kolizji z przeszkodami (a jeżeli jest to konieczne, także z uwzględnieniem innych ograniczeń [186]), traktowana jest jako zbiór dopuszczalnych dyskretnych konfiguracji robota x Rfree. Wzdłuż tej ścieżki w równych odległościach generowana jest określona liczba konfiguracji x Ri P nom, i = 1...n, reprezentujących te pozycje robota, w których może on podejmować akcje pozycjonujące (nie we wszystkich pozycjach jest to możliwe). Odległość między kolejnymi pozycjami (liczona wzdłuż ścieżki) powinna być niewielka; im jest ona mniejsza, tym więcej potencjalnych akcji pozycjonujących będzie wygenerowanych, lecz zwiększy się też koszt przeszukiwania grafu akcji. Akcja v i w i-tej pozycji na ścieżce jest określona przez tę pozycję x Ri, rodzaj pozycjonowania (kamera pokładowa lub agent percepcyjny), macierz kowariancji C Si opisującą niepewność pozycji wynikającą z użycia danego typu sensora (rodzaju akcji) w tej konfiguracji robota, oraz koszt pozycjonowania T i. Macierz kowariancji w zależności od typu akcji jest wyznaczana za pomocą formuł podanych w podrozdziale 2.5. Koszt jest dodatnią liczbą całkowitą uzyskaną z tabeli zawierającej określone eksperymentalnie czasy pozycjonowania w zależności od typu akcji (akwizycja obrazu z kamery pokładowej, komunikacja z agentem percepcyjnym) i konfiguracji robota. Wygenerowane akcje pozycjonujące są wierzchołkami grafu G(V,E), V = n i=1 v i (rys. 6.1B). Wierzchołek v i jest połączony z wierzchołkiem v j krawędzią e i,j E, jeżeli jest możliwe przemieszczenie się robota mobilnego z x Ri do x Rj z zachowaniem niepewności położenia poniżej danego progu określonego przez pole powierzchni elipsy C max (wielkość skalarna). 224

Nie nałożono ograniczenia na niepewność orientacji robota. Ponieważ jednak niepewność położenia rozważanego robota o różnicowym układzie jezdnym jest związana z niepewnością orientacji poprzez kinematykę pojazdu, to utrzymanie niepewności położenia poniżej określonego progu oznacza także, że ograniczana jest niepewność orientacji. Niepewność położenia jest istotna z uwagi na bezpieczeństwo pokonywania bezkolizyjnej ścieżki, natomiast ewentualne błędy orientacji mogą być skorygowane przez obrót robota w miejscu. Ponieważ założono, że niepewność może być redukowana jedynie w wyniku akcji pozycjonujących (w wierzchołkach grafu), do określenia maksymalnej odległości między połączonymi wierzchołkami wykorzystuje się model odometrii robota typu Labmate przedstawiony w podrozdziale 2.2. i-ta O 1 akcja O 2 C s,i y start - uk³ad globalny C A C i x E y j-ta akcja B x C i,j C j,k y x F O 3 y akcje k i k C j 1 2 D G x C k1 H C k2 x y Rysunek 6.2. Źródła niepewności przykładowych akcji pozycjonujących W grafie akcji może być wiele wierzchołków o tej samej konfiguracji robota x R, lecz różnych macierzach C S i/lub koszcie T. Wynika to z faktu, że w niektórych punktach nominalnej ścieżki robot może podjąć kilka różnych akcji pozycjonujących, np. w polu widzenia robota znajduje się sztuczny znacznik, a jednocześnie robot jest widziany przez kamerę monitorującą. Ponieważ przyjęto (zgodnie z konfiguracją stanowiska eksperymentalnego), że kamera pokładowa jest sztywno związana z robotem, gdy robot zmienia orientację (obraca się w miejscu), widząc znacznik, kilka akcji pozycjonujących może być wygenerowanych w tym samym punkcie, lecz będą się one różniły orientacją i pozostałymi parametrami. Z krawędziami grafu są skojarzone koszty akcji pozycjonujących. Krawędź e i,j ma koszt T j, ponieważ z założenia przejście do danego wierzchołka (j-tego) pociąga za sobą wykonanie związanej z nim akcji pozycjonującej i poniesienie jej kosztów. Z krawędziami wiąże się też niepewność przemieszczenia się między wierzchołkami. Dla krawędzi e i,j macierz C i,j odwzorowuje zwiększenie niepewności spowodowane błędami odometrii. Niepewność wzdłuż ścieżki, którą podąża robot, wykonując akcje pozycjonujące, jest wyznaczana w wyniku operacji złożenia (oznaczonych ) niepewności pozycji robota z ko- 225

lejnymi macierzami niepewności otrzymanymi z odometrii oraz operacji łączenia (oznaczonych ) otrzymanych nowych macierzy z niepewnością akcji pozycjonujących. Wykorzystano operacje składania i łączenia niepewności zdefiniowane w pracy [289] (por. p. 4.3). Operacja złożenia dla wierzchołków v i oraz v j dana jest wzorami: ˆx i,j cos ˆθ i ŷ i,j sin ˆθ i + ˆx i ˆx R j = ˆx Ri ˆx Ri,j = ˆx i,j sin ˆθ i + ŷ i,j cos ˆθ i + ŷ i, (6.1) ˆθ i + ˆθ i,j C R j = J 1 C Ri J T 1 + J 2 C i,j J T 2, (6.2) gdzie ˆx R j jest estymatą pozycji robota po osiągnięciu j-tego wierzchołka, a przed wykonaniem związanej z nim akcji pozycjonującej, C R j jest niepewnością tej estymaty, a jakobiany przyjmują postać: 1 0 ˆx i,j sin ˆθ i ŷ i,j cos ˆθ i cos ˆθ i sin ˆθ i 0 J 1 = 0 1 ˆx i,j cos ˆθ i ŷ i,j sin ˆθ i, J 2 = sin ˆθ i cos ˆθ i 0. (6.3) 0 0 1 0 0 1 Operacja łączenia istniejącej informacji o pozycji robota w j-tym wierzchołku (ˆx R j, C R j ) z informacją będącą wynikiem wykonania związanej z tym wierzchołkiem akcji pozycjonującej (ˆx Sj, C Sj ) odbywa się za pomocą statycznego filtru Kalmana: ˆx Rj ) = ˆx R j ˆx Sj = ˆx R j K (ˆx Rj ˆx Sj, (6.4) K = C R j (C R j + C Sj ) 1, (6.5) C Rj = (I K)C R j. (6.6) Źródła niepewności poszczególnych akcji pozycjonujących przedstawiono na rys. 6.2, na którym literami A... D oznaczono pozycje elementów infrastruktury zewnętrznej względem układu globalnego, literami E... H pozycje robota względem kamer monitorujących i sztucznych znaczników, a O 1,O 2,O 3 przemieszczenia robota pod kontrolą odometrii. Na przykład, dla j-tej akcji z rys. 6.2 niepewność pozycji robota przed wykonaniem tej akcji (rozpoznaniem znacznika) można symbolicznie wyrazić jako (O 1 (A E)) O 2. Powstały graf akcji G(V, E) jest grafem skierowanym, gdyż akcje mogą być wykonywane jedynie w określonej kolejności przez robota poruszającego się wzdłuż ścieżki P nom od startu v s do celu v g. Graf ten jest acykliczny, ponieważ każda akcja może być wykonana tylko raz, przy założeniu, że robot nie może zawrócić, a ścieżka nominalna nie tworzy pętli. 6.3. Metody planowania sekwencji akcji To, czy robot pokona drogę odpowiadającą danej krawędzi e k,l grafu G(V,E), nie naruszając ograniczenia niepewności, nie zależy tylko od niepewności C Sk akcji pozycjonującej związanej z wierzchołkiem v k będącym początkiem tej krawędzi oraz od tego, na ile niepewność ta będzie zwiększona przez skojarzoną z e k,l niepewność odometrii C k,l, lecz także od początkowej niepewności położenia, osiąganej przez robota docierającego do v k. Niepewność 226

położenia robota w v k (przed pozycjonowaniem) zależy od tego, jaką ścieżką podążał od wierzchołka v s do v k (ile krawędzi grafu pokonał i jak na jego niepewność wpłynęła skojarzona z krawędziami niepewność odometrii), oraz od tego jakie akcje pozycjonujące wykonał (które mogły poprawić estymatę jego położenia). Początkowa niepewność w danym wierzchołku jest trudna do wyznaczenia, gdyż wymaga to dynamicznego śledzenia zmian macierzy kowariancji symulowanego robota przemieszczającego się w grafie akcji podczas planowania drogi w tym grafie. Proste poszukiwanie najkrótszej drogi w opisanym powyżej grafie akcji (np. za pomocą algorytmu Dijkstry [295]) pozwala znaleźć sekwencję akcji o najmniejszym koszcie całkowitym. Taka sekwencja akcji nie pozwala jednak osiągnąć założonego celu utrzymania miary niepewności położenia robota poniżej dopuszczalnej wartości C max. W niektórych pracach poświęconych planowaniu ścieżki robota mobilnego z uwzględnieniem niepewności wyszukiwanie drogi w grafie przy istniejącym ograniczeniu rozwiązane jest przez zdefiniowanie grafu, którego krawędzie mają zmodyfikowane wagi, będące pewną liniową kombinacją rzeczywistego kosztu pokonania danej krawędzi (np. odległość, czas), oraz skalarnej miary niepewności [110, 176]. Odpowiednie współczynniki skalujące pozwalają uzyskać kompromis między obiema wielkościami, mającymi odmienne znaczenie fizyczne. Taki pośredni sposób uzyskania optymalnej (czy też suboptymalnej) ścieżki z uwzględnieniem niepewności nie pozwala jednak na dokładne przestrzeganie ograniczenia, a ustawienie odpowiednich współczynników skalujących może wymagać wielu prób. Prostym rozwiązaniem problemu planowania akcji pozycjonujących przy założeniu dokładnego przestrzegania ograniczenia dopuszczalnej niepewności położenia jest zbudowanie takiego grafu akcji, w którym wszystkie możliwe ścieżki od wierzchołka startowego v s do wierzchołka docelowego v g gwarantują utrzymanie niepewności położenia na żądanym poziomie. Wówczas poszukiwanie optymalnej sekwencji akcji sprowadza się do znalezienia najkrótszej drogi w grafie akcji, przy czym pod uwagę bierze się jedynie wagi krawędzi T, czyli koszt pozycjonowania. Graf taki można zbudować z wykorzystaniem własności filtru Kalmana, który jest używany do łączenia w sposób optymalny nowych estymat pozycji (tj. wyników akcji pozycjonujących) z bieżącą estymatą pozycji robota. Użycie tego filtru sprawia, że otrzymana estymata pozycji nie będzie mieć niepewności większej niż najlepsza (lepsza z dwóch dostępnych) estymata pozycji na jego wejściu [256]. Ta własność filtru Kalmana pozwala robotowi integrować nowe dane bez obaw, że nieprecyzyjny pomiar może pogorszyć istniejącą informację o jego pozycji, zakładając oczywiście, że każdy pomiar ma prawidłowo określoną niepewność. Ponieważ znana jest niepewność akcji pozycjonującej podejmowanej w rozważanym wierzchołku, wykorzystując powyższą własność filtru Kalmana, można założyć, że końcowa niepewność (po pozycjonowaniu) w v k jest nie większa niż C Sk. Następnie należy zbudować graf G C = (V,E C ) (rys. 6.3A), w którym dwa dane wierzchołki v k i v l są połączone krawędzią e k,l E C tylko wtedy, gdy miara niepewności akcji pozycjonującej w v k, powiększonej o niepewność C k,l wynikającą z użycia odometrii do określania pozycji wzdłuż ścieżki odpowiadającej e k,l, jest mniejsza niż C max. W wyniku wykorzystania dowolnej znanej metody poszukiwania najkrótszej drogi ze źródła v s do wierzchołka v g względem dodatnich wag krawędzi w grafie G C uzyska się sekwencję wierzchołków (akcji pozycjonujących) optymalną pod względem kosztu i zapewniającą zachowanie niepewności położenia robota poniżej założonego progu. Opisane wyżej podejście można określić mianem zachowawczego, ponieważ zakłada się w nim występowanie największej z możliwych (lecz z góry znanej) niepewności w każdym wierzchołku grafu akcji. Stosując takie podejście w planowaniu akcji, uzyskuje się rozwiązanie w rzeczywistości suboptymalne (chociaż optymalne w grafie G C ), gdyż może ist- 227

nieć sekwencja wierzchołków pewnego grafu G(V, E) o tych samych wierzchołkach V, lecz inaczej wyznaczonych krawędziach, która przeprowadzi robota od v s do v g przy mniejszym koszcie, respektując ograniczenie C max. Wynika to z nieuwzględnienia w opisanym podejściu możliwości polepszenia wyjściowej estymaty pozycji robota w danym wierzchołku v k za pomocą akcji pozycjonujących, które robot wykonał pomiędzy startem v s a wierzchołkiem v k. W takiej sytuacji wyjściowa (po pozycjonowaniu) niepewność w v k może być mniejsza niż C Sk. Mając mniejszą niepewność wyjściową, robot może podjąć kolejną akcję pozycjonującą w punkcie bardziej odległym od v k. Graf akcji umożliwiający poszukiwanie sekwencji akcji z uwzględnieniem rzeczywistej niepewności, którą ma robot w danym wierzchołku, może być skonstruowany z użyciem jako niepewności wyjściowej danego wierzchołka v k (po pozycjonowaniu) niepewności mniejszej niż C Sk. Najmniejszą niepewność, którą robot może mieć podczas poruszania się po nominalnej ścieżce, określa najlepsza akcja pozycjonująca w całym grafie akcji, tj. macierz C Si opisująca najmniejszą elipsę niepewności. Jeżeli ta macierz będzie użyta w każdym wierzchołku zamiast macierzy akcji przypisanej do tego wierzchołka, wynikowy graf akcji G D = (V,E D ) będzie miał więcej krawędzi (rys. 6.3B), ponieważ startujący z mniejszą niepewnością robot może, wykorzystując odometrię, dojechać dalej, bez przekroczenia założonego progu niepewności C max. Na rysunku 6.3 schematycznie porównano oba rodzaje grafów. Optymalna sekwencja wierzchołków w 1 2 G C(V,E C) 1 2 G D(V,E D) 3 3 4 5 4 5 Rysunek 6.3. Grafy akcji wygenerowane różnymi metodami grafie G D z uwzględnieniem ograniczenia C max nie może być poszukiwana z wykorzystaniem metody Dijkstry lub innego podobnego algorytmu, ponieważ ograniczenie niepewności położenia musi być uwzględnione dynamicznie w procedurze poszukiwania [279]. Z literatury wiadomo, że poszukiwanie najkrótszej drogi w grafie z dodatkowym ograniczeniem (ang. Restricted Shortest Path problem RSP) należy do klasy problemów NP-trudnych [8]. Opracowano jednak kilka efektywnych algorytmów umożliwiających przybliżone rozwiązanie tego zagadnienia w czasie wielomianowym (ang. Fully Polynomial Approximation Scheme FPAS). Powstają także pakiety oprogramowania (procedur) ułatwiające zastosowanie algorytmów RSP w rozwiązywaniu różnych praktycznych problemów dających się sprowadzić do tego zagadnienia optymalizacji dyskretnej [205]. Powodem wzrostu zainteresowania RSP jest głównie jego przydatność do rozwiązywania zagadnień efektywnego routingu w aplikacjach nakładających ograniczenia czasowe na transfer danych [174]. Poniżej (algorytm 6.1) przedstawiono propozycję algorytmu rozwiązującego zagadnienie planowania akcji z ograniczeniem potraktowane jako problem RSP. Jest to algorytm o pseudowielomianowej złożoności obliczeniowej, tzn. czas jego wykonania zależy nie tylko od rozmiarów grafu, lecz także od rzeczywistych wartości danych wejściowych (długości ścieżki) [231]. Istnieje możliwość zmodyfikowania tej metody przez zastosowanie aproksymacji o złożoności wielomianowej (FPAS) zaproponowanej w artykule [185], jeżeli efektywność podstawowego algorytmu nie będzie wystarczająca. 228

Algorytm 6.1 procedure PLANNINGBYRSP(G(V,E), v s, v g, C max, T max) 1 begin 2 for each v V {v s} C[v, 0]:= ; {inicjacja} 3 C[v s, 0]:=0; {inicjacja} 4 for t := 1 to T max do 5 begin 6 for each v V do 7 begin 8 C[v, t]:=c[v, t 1] ; P[v, t]:=nil 9 end; 10 for each e u,v E do 11 begin 12 C temp:=(c[u, t T u,v] C u) C u,v; {aktualizacja niepewności} 13 if C temp < C[v, t] and C temp C max then 14 begin 15 C[v, t]:=c temp ; P[v, t]:=u {aktualizacja ścieżki} 16 end; 17 if C[v g, t] C max then return P[v g, t]; {znaleziono sekwencję} 18 end; 19 end; 20 fail; {przekroczone T max} 21 end. W algorytmie 6.1 C[v, t] oznacza wektor skojarzony z każdym wierzchołkiem v, przechowujący najmniejszą niepewność pozycji robota na każdej drodze z v s do v, której koszt przejścia jest równy t. Maksymalny koszt drogi z v s do v g w grafie akcji, wyznaczonym w wyniku poszukiwania uwzględniającego tylko koszty krawędzi grafu [295], oznaczono T max. Jest to wartość określająca warunek zatrzymania algorytmu 6.1. Macierz C u,v reprezentuje niepewność spowodowaną przez odometrię podczas pokonywania odcinka skojarzonego z krawędzią e u,v, a C u jest niepewnością akcji pozycjonującej w wierzchołku u. Notacja C oznacza obliczenie skalarnej wielkości będącej miarą niepewności opisanej daną macierzą kowariancji (jest to pole elipsy). Niepewność jest wyznaczana w wyniku operacji złożenia następujących kolejno po sobie macierzy niepewności krawędzi C i,j oraz operacji łączenia otrzymanych rezultatów z macierzami niepewności akcji pozycjonujących w kolejnych wierzchołkach C Si. 6.4. Eksperymentalna weryfikacja metody planowania akcji Dotychczas przetestowano eksperymentalnie procedurę planowania akcji pozycjonujących opartą na podejściu zachowawczym. W celu sprawdzenia zaproponowanej metody przeprowadzono eksperyment z robotem Labmate wyposażonym w kamerę pokładową do obserwacji sztucznych znaczników rozmieszczonych w laboratorium i znaczniki diodowe umożliwiające współpracę z kamerami monitorującymi. Robot podążał wyznaczoną (nominalną) ścieżką, podejmując akcje pozycjonujące w postaci obserwacji rozmieszczonych w laboratorium trzech sztucznych znaczników oraz komu- 229

A B C D v 1 n n E F v n v 1 v n G H Rysunek 6.4. Wyniki eksperymentu zaplanowana sekwencja akcji (opis w tekście) nikacji z agentami percepcyjnymi dwóch kamer monitorujących. Akcje pozycjonujące zostały zaplanowane przed eksperymentem (off-line) na podstawie znajomości nominalnej ścieżki, pozycji kamer monitorujących i rozmieszczenia znaczników, zgodnie z procedurą opisaną w 230