Politechnika Poznańska Wydział Informatyki i Zarządzania Katedra Sterowania i Inżynierii Systemów

Transkrypt

Politechnika Poznańska
Wydział Informatyki i Zarządzania
Katedra Sterowania i Inżynierii Systemów
Praca dyplomowa magisterska
LOKALIZACJA ROBOTA MOBILNEGO I BUDOWA
MAPY OTOCZENIA Z WYKORZYSTANIEM SKANERA
LASEROWEGO
Mateusz Przybyła
Promotor
dr inż. Dariusz Pazderski
Poznań, 2010 r.
Tutaj przychodzi karta pracy dyplomowej;
oryginał wstawiamy do wersji dla archiwum PP, w pozostałych kopiach wstawiamy ksero.
Pracę dedykuję wszystkim tym, którzy dają
mi motywację i wiarę w to, że rozwój technologii
może uczynić Świat lepszym, i którzy sami głęboko
w to wierzą.
Spis treści
1 Wstęp
1
1.1
Przedstawienie problemu, cele i założenia pracy . . . . . . . . . . . . . . . .
1
1.2
Motywacja i uzasadnienie podjęcia tematu . . . . . . . . . . . . . . . . . . .
2
1.3
Problemy związane z realizacją tematu . . . . . . . . . . . . . . . . . . . . .
3
1.4
Opis rozdziałów . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
3
2 Robot Mobilny typu AGV
5
2.1
Kinematyka typu (2,0) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6
2.2
Odometria pojazdu o kinematyce (2,0) . . . . . . . . . . . . . . . . . . . . .
7
2.2.1
Zasada wykonywania pomiaru . . . . . . . . . . . . . . . . . . . . . .
7
2.2.2
Błędy systematyczne i losowe odometrii . . . . . . . . . . . . . . . .
8
2.3
Rozmieszczenie dalmierza laserowego na robocie
. . . . . . . . . . . . . . .
3 Przegląd sensorów odległości
3.1
3.2
3.3
8
10
Czujniki triangulacyjne, wykorzystujące światło niekoherentne . . . . . . . . 10
3.1.1
Zasada działania . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
3.1.2
Charakter błędów i problemy związane z pomiarem . . . . . . . . . . 11
Sonary ultradźwiękowe . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
3.2.1
3.2.2
Charakter błędów i problemy związane z pomiarem . . . . . . . . . . 12
Skanery laserowe . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
3.3.1
3.3.2
Źródła błędów systematycznych i losowych . . . . . . . . . . . . . . 15
3.3.3
Skaner LMS200 firmy SICK . . . . . . . . . . . . . . . . . . . . . . . 16
4 Sposoby reprezentacji środowiska
18
4.1
Mapa rastrowa . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18
4.2
Mapa wektorowa . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
4.3
Mapa topologiczna . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
5 Środowisko symulacyjne procesu skanowania otoczenia
5.1
22
Model skanera laserowego . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
I
II
5.2
Model wirtualnego otoczenia . . . . . . . . . . . . . . . . . . . . . . . . . . 23
5.3
Proces pomiarowy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
6 Ekstrakcja cech geometrycznych z pojedynczego skanu
26
6.1
Grupowanie . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26
6.2
Segmentacja . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28
6.3
Analiza statystyczna odcinków . . . . . . . . . . . . . . . . . . . . . . . . . 30
6.4
Rekonstrukcja prymitywów na poziomie analizy skanu . . . . . . . . . . . . 32
6.4.1
Łatanie dziur . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32
6.4.2
Poprawa i rejestracja narożników . . . . . . . . . . . . . . . . . . . . 33
7 Budowa mapy globalnej przy pełnej znajomości konfiguracji robota
36
7.1
Rzutowanie uzyskanych prymitywów . . . . . . . . . . . . . . . . . . . . . . 36
7.2
Analiza podobieństw między prymitywami . . . . . . . . . . . . . . . . . . . 37
7.3
Rekonstrukcja mapy globalnej . . . . . . . . . . . . . . . . . . . . . . . . . . 39
7.4
Wyniki eksperymentalne . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40
7.4.1
Badania symulacyjne . . . . . . . . . . . . . . . . . . . . . . . . . . . 40
7.4.2
Badania na obiekcie rzeczywistym . . . . . . . . . . . . . . . . . . . 41
8 Lokalizacja globalna przy pełnej znajomości mapy otoczenia
43
8.1
Odcinki w pełni widzialne . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43
8.2
Relacje geometryczne mapy lokalnej . . . . . . . . . . . . . . . . . . . . . . 45
8.3
Estymacja konfiguracji skanera laserowego . . . . . . . . . . . . . . . . . . . 48
8.4
Zawężenie zakresu pomiaru na podstawie wiedzy a priori . . . . . . . . . . . 49
8.5
Wyniki eksperymentalne . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50
8.6
8.5.1
Badania symulacyjne . . . . . . . . . . . . . . . . . . . . . . . . . . . 50
8.5.2
Badania na obiekcie rzeczywistym . . . . . . . . . . . . . . . . . . . 51
Problemy związane z lokalizacją globalną . . . . . . . . . . . . . . . . . . . 52
9 Jednoczesna lokalizacja i mapowanie
55
9.1
Robot wyposażony w skaner laserowy i odometrię . . . . . . . . . . . . . . . 55
9.2
Robot wyposażony jedynie w skaner laserowy . . . . . . . . . . . . . . . . . 56
9.3
Optymalizacja procesu SLAM . . . . . . . . . . . . . . . . . . . . . . . . . . 57
10 Podsumowanie
59
10.1 Wnioski . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59
10.2 Dalszy rozwój projektu . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60
A Problemy związane z okresowością kąta
62
B Obsługa środowiska symulacyjnego
64
C Praca ze skanerem laserowym SICK LMS200
65
III
Literatura
68
Rozdział 1
Wstęp
1.1
Przedstawienie problemu, cele i założenia pracy
Lokalizacja robota mobilnego w przestrzeni roboczej jest zwykle kluczowym elementem
zapewniającym możliwość wykonywania powierzanych mu zadań. Komplementarna w
stosunku do powyższego jest znajomość otoczenia, w którym przyjdzie robotowi pracować, gdyż lokalizacja musi być zawsze wykonywana względem odpowiedniego układu
odniesienia. Im większą ilość danych o środowisku posiada robot, tym dokładniej potrafi
się zlokalizować (tzw. samolokalizacja) i wykonać zadanie, np. transportu, inspekcji, konserwacji, itp.
O wymienionych kwestiach przekonywało się na przestrzeli lat coraz większe grono
naukowców i inżynierów z całego świata. Współcześnie jednym z kluczowych zagadnień
jest „SLAM” (ang. Simultaneous Localisation and Mapping - ustawiczna lokalizacja i
mapowanie). Problematyka ta początkowo rozwijana była przez Durrant-Whyte’a i J.
Leonarda [BDW94, LDW91b]. Podana metoda polega na zaopatrzeniu robota mobilnego
w narzędzie generujące mapę otoczenia i jednocześnie estymujące jego pozycję za pomocą
fuzji danych nawigacyjnych z wykorzystaniem EKF (ang. Extended Kalman Filter - rozszerzony filtr Kalmana) [LDW91a].
Zarówno budowa mapy jak i proces lokalizacji wymagają zamontowania na pokładzie
robota odpowiednich sensorów. Na podstawie zwracanych przez nie sygnałów kształtowany jest model numeryczny otaczającego świata. Najczęściej wykorzystywane są w
tym celu czujniki odległości, przede wszystkim skanery laserowe, do celów uzupełniających
stosuje się także dalmierze ultradźwiękowe. Spotyka się również rozwiązania wykorzystujące sensory wizyjne.
Niniejsza praca przedstawia szczegółowo proces przetwarzania surowych danych otrzymywanych ze skanera laserowego, ich późniejszą analizę i ekstrakcję cech używanych do bu-
1
1.2. Motywacja i uzasadnienie podjęcia tematu
2
dowy i aktualizacji mapy otoczenia oraz przykładowe rozwiązanie problemu samolokalizacji na podstawie pojedynczego pomiaru (skanu) i znajomości mapy otoczenia. Praca może
być postrzegana jako wprowadzenie do zagadnienia jednoczesnej budowy mapy i ustawicznej lokalizacji. Problem w głównej mierze rozpatrywano w oparciu o przygotowane
w Matlabie (wersja 7.0.1) środowisko symulacyjne. Dodatkowo przeprowadzono badania algorytmu wykorzystując dane eksperymentalne dostarczone przez skaner laserowy w
statycznym środowisku rzeczywistym. Prace wykonywane były na komputerze z systemem
Windows XP i procesorem Intel Celeron M420 1,60GHz.
1.2
Motywacja i uzasadnienie podjęcia tematu
Praca powstała ze względu na duże znaczenie narzędzi lokalizacyjnych dla robotyki
mobilnej. Określanie pozycji na podstawie mapy daje precyzyjne wyniki bez konieczności
ingerowania w środowisko. Mapa, powstała jako rezultat działania algorytmu, może być
później wykorzystywana przez inne roboty poruszające się na tym samym terenie. Połączenie obu procesów (mapowania i lokalizacji) jest praktycznym narzędziem pozwalającym
uzyskać dużą autonomię robota mobilnego (zarówno naziemnego, powietrznego, a potencjalnie także na- i podwodnego). Może ono zostać użyte przy okazji wykonywania
elementarnych zadań jak: parkowanie, omijanie przeszkód, odnajdywanie bezkolizyjnych
ścieżek, generowanie trajektorii ruchu on-line, inspekcja pomieszczeń, itp.
Na zastosowanie skanera laserowego do realizacji projektu zdecydowano się głównie ze
względu na fakt, że dostępne dziś urządzenia oferują dużą precyzję i krótki czas wykonywania pomiaru. Ponadto ilość próbek odległości zawartych w jednym skanie znacząco
przewyższa możliwości związane z zastosowaniem szeregu pojedynczych dalmierzy. Wykorzystanie wizji (lub stereowizji) wymaga wielokrotnie większych nakładów obliczeniowych
oraz generuje wiele problemów z dziedziny widzenia maszynowego. Co więcej, w pracy
skupiono się jedynie na przypadku otoczenia dwuwymiarowego, gdzie zastosowanie wizji
jest wysoce redundantne.
Dobrze zaprojektowany system mapowania i lokalizacji pozwala na implementację w
czasie rzeczywistym, co omija wymóg wstrzymania pracy robota na potrzeby percepcyjne (ang. stop-look-update process). Z tego względu system taki nie tylko zapewnia
poprawną samolokalizację, ale także zabezpiecza przed niepożądaną kolizją w dynamicznych środowiskach. Właściwa analiza danych otrzymywanych ze skanera może bowiem
spełniać jego pierwotne zadanie, czyli funkcję bariery świetlnej.
1.3. Problemy związane z realizacją tematu
1.3
3
Problemy związane z realizacją tematu
Głównymi trudnościami, z którymi wiąże się realizacja mapowania i lokalizacji jest
problem dopasowania cech uzyskanych ze skanu do tych istniejących już na mapie. Proces ten wymaga określenia warunków, które w możliwie jednoznaczny sposób pozwolą na
klasyfikację podobieństwa dwóch prymitywów geometrycznych z uwzględnieniem przyjętego
przedziału ufności (dopuszczalnych odchyleń) dla każdej wartości porównawczej. Wiąże
się to z projektowaniem złożonych algorytmów oraz ich wielokrotnym, często żmudnym
testowaniem i strojeniem. Od poprawnego dopasowania cech map: lokalnej i globalnej
zależy skuteczność i jakość algorytmu, dlatego warunki klasyfikacji powinny być bardzo
rygorystyczne, a zabezpieczenia kładzione na przypadki błędnej identyfikacji zwielokrotnione.
Problemem innej natury jest sposób działania wykorzystanych dalmierzy i wynikające z tego ograniczenia. W przypadku skanerów laserowych, mimo że ich precyzja jest
duża, istnieją sytuacje, w których użyteczność jest wątpliwa. Wymienić tutaj można, np.
drucianą siatkę, przez którą robot nie przejedzie, a którą z dużym prawdopodobieństwem promienie lasera mogą „przeoczyć”. Innym trudnym elementem środowiska jest
powierzchnia lustrzana, która ustawiona pod niezerowym kątem do promienia odbija go, co
skutkuje znikomą energią fali odbitej. Także obiekty przeźroczyste stanowią spory kłopot.
Środowiska o różnych gęstościach optycznych zgodnie z prawem Snella powodują ugięcie
promieni świetlnych w momencie przejścia z jednego do drugiego co może przekłamywać
wynik pomiaru.
1.4
Opis rozdziałów
W rozdziale drugim znajduje się opis urządzenia jakim jest robot mobilny, przedstawiono podział jednostek naziemnych ze względu na mobilność oraz krótki opis kinematyki
robota (2,0). Omówiono w nim także problem odometrii oraz rozmieszczenie skanera
laserowego w lokalnym układzie współrzędnych związanym z robotem.
Rozdział trzeci zawiera informacje na temat sensorów odległości takich jak czujniki
podczerwieni, sonary ultradźwiękowe oraz skanery laserowe. Scharakteryzowano w nim
także pokrótce skaner LMS200 firmy SICK wykorzystany w celach badawczych.
Rozdział czwarty opisuje przykładowe sposoby reprezentacji środowiska w formie mapy
i wskazuje na ich podstawowe właściwości.
W rozdziale piątym zawarto opis zbudowanego do badań i celów dydaktycznych środowiska
symulacyjnego oraz przedstawiono sposób w jaki realizuje wirtualny pomiar.
1.4. Opis rozdziałów
4
Kolejny, szósty rozdział zawiera informacje dotyczące problemu ekstrakcji cech geometrycznych z analizowanej sekwencji pomiarowej i sposoby ich rekonstrukcji na podstawie
statystyki oraz wzajemnych relacji geometrycznych. Opisuje on poszczególne kroki odnajdywania odcinków i narożników, tworzących numeryczny obraz świata.
Rozdział siódmy przedstawia proces budowy mapy globalnej przy znanym stanie robota, omawia sposób rekonstrukcji wchodzących w skład mapy prymitywów i prezentuje
wyniki eksperymentalne otrzymane w wyniku testowania algorytmu.
Ósmy rozdział dotyczy lokalizacji globalnej przy założeniu znajomości mapy otoczenia.
Przedstawia sposób klasyfikacji relacji pomiędzy dwoma cechami oraz omawia sposób zawężenia pola przeszukiwania przy zgrubnej wiedzy apriorycznej. Także prezentuje wyniki
eksperymentalne w tym zakresie.
W rozdziale dziewiątym zawarto informacje będące wstępem do zagadnienia SLAM,
czyli ustawicznej lokalizacji i mapowania.
Rozdział dziesiąty przedstawia ostateczne wnioski uzyskane w trakcie pracy oraz wskazówki
odnośnie dalszego rozwoju projektu.
Rozdział 2
Robot Mobilny typu AGV
Robot mobilny to urządzenie techniczne, które potrafi zmieniać swoje położenie w
przestrzeni zgodnie z określonym programem. Wyróżnia się zarówno jednostki na- i podwodne (USV - ang. Unmanned Surface Vehicles - bezzałogowe pojazdy nadwodne, UUV
- ang. Unmanned Underwater Vehicles - bezzałogowe pojazdy podwodne), powietrzne
(UAV - ang. Unmanned Aerial Vehicles - bezzałogowe pojazdy powietrzne) jak i lądowe (UGV - ang. Unmanned Ground Vehicles - bezzałogowe pojazdy naziemne). Innymi słowy: jednostki pływające, latające lub jeżdżące, względnie kroczące. Na łamach
niniejszej pracy rozpatrywany będzie jedynie typ lądowy.
Postęp techniki dąży do tego, aby jednostkę taką można było określić mianem AGV
(ang. Autonomous Ground Vehicle - niezależny pojazd lądowy), czyli potrafiącą samodzielnie podejmować decyzje i odpowiednie kroki tak, aby wykonać powierzone zadanie. Pewną
ograniczoną implementacją tej idei są pojazdy wykorzystywane w przemyśle do transportu
narzędzi, surowców, półproduktów lub wyrobów końcowych. Mówi się wówczas o tzw.
„Automated Guided Vehicles” (automatycznie prowadzone pojazdy). Pomimo pewnego
stopnia samodzielności, pojazdy te mają ściśle określoną przestrzeń roboczą, oznaczoną
za pomocą linii naziemnych lub innego rodzaju landmarków. Przykładem, który wskazuje
na postępy w zwiększaniu autonomii są zawody DARPA (ang. Defense Advanced Research Project Agency) Grand oraz Urban Challenge, w których startujące drużyny przygotowują pełnowymiarowy samochód mający za zadanie przejazd odpowiedniej trasy w
rzeczywistym świecie [dar09b, dar09a].
Jednostki naziemne podzielić można ze względu na sposób poruszania się na kroczące,
ślizgające się (np. czołg) lub toczące się. Z punktu widzenia praktycznego największe
znaczenie mają roboty mobilne toczące się, czyli poruszające się na kołach, gdyż są one
relatywnie proste w realizacji i zapewniają energetycznie efektywny ruch. W zależności od
liczby, rozmieszczenia, oraz funkcjonalności zamontowanych kół wyróżnia się m.in. kinematyki: [GC96]
5
2.1. Kinematyka typu (2,0)
6
• (3,0) - tzw. omnibot, posiada pełną mobilność w ruchu na płaszczyźnie. Konstrukcja
opiera się, np. na trzech kołach szwedzkich typu 90o lub czterech typu 45o , tudzież
napędach kulowych.
• (2,1) - tzw. omni-steer, platforma mogąca dowolnie poruszać się na płaszczyźnie,
ale bez zmiany orientacji. Sterowanie orientacją polega na obracaniu wieżyczki zamontowanej na platformie.
• (2,0) - tzw. układ różnicowy (ang. differential drive), występuje ograniczenie ruchu
w poprzek. Konstrukcja opiera się na dwóch kołach napędowych o wspólnej osi
obrotu.
• (1,2) - bi-steerable car, pojazd o parze sprzężonych kół skrętnych z przodu i z tyłu,
przy czym jedna z nich jest dodatkowo napędzana.
• (1,1) - samochód, dwa sprzężone koła ustalone z tyłu i skrętne z przodu. W zależności od konstrukcji jedna z tych par jest napędzana. Koła przednie sprzężone
są mechanizmem Ackermana, natomiast na osi kół tylnych zamontowany jest tzw.
element różnicowy.
Podział ten dotyczy jedynie platform, których kinematyka strukturalnie pozwala na ruch
kół bez poślizgu. Jedną z prostszych w realizacji i często stosowanych w robotyce jest
robot mobilny o strukturze kinematycznej (2,0).
2.1
Kinematyka typu (2,0)
Robot tego typu posiada dwa jednakowe napędzane koła o ustalonej wspólnej osi.
Jest to tzw. układ różnicowy. Kinematykę (2,0) charakteryzuje „naturalne” ograniczenie ruchu w poprzek, co pozwala zaklasyfikować pojazdy tego typu do grupy robotów
nieholonomicznych. Łatwo wyobrazić sobie, że gdy koła obracać się będą z taką samą
prędkością, lecz w przeciwnym kierunku, pojazd będzie obracał się w miejscu. Przy tej
samej prędkości obrotowej i tym samym zwrocie, pojazd porusza się po linii prostej. Opis
ruchu robota mobilnego o kinematyce (2,0) jest zatem bardzo intuicyjny.
Określenie modelu takiego pojazdu (rys. 2.1) wymaga założenia charakterystycznego
punktu P , będącego początkiem lokalnego układu odniesienia. Z reguły przyjmuje się,
że punkt ten leży na osi kół dokładnie pomiędzy nimi i w idealnym przypadku pokrywa
się ze środkiem geometrycznym pojazdu. W praktyce pojazd wyposażony jest w podpory
z przodu i z tyłu redukujące odchylenia wokół osi poprzecznej (w aeronautyce nazywane
jako kąt pitch). Mogą być to ślizgacze lub koła pasywne (np. koło Kastora lub kula).
7
2.2. Odometria pojazdu o kinematyce (2,0)
Rysunek 2.1: Kinematyka robota klasy (2,0).
Konfigurację robota można zdefiniować jako ~qr = [θr Xr Yr ]T , gdzie współrzędne Xr
i Yr określają współrzędne punktu P , zaś θr oznacza orientację robota względem układu
podstawowego.
2.2
Odometria pojazdu o kinematyce (2,0)
Odometria polega na estymacji konfiguracji robota na podstawie zmierzonej drogi kątowej kół. Jest jednym z przykładów realizacji tzw. Dead Reckoning’u, czyli całkowania
ścieżki. Metoda ta wymaga dyskretnego sumowania przebytych odcinków trasy, dlatego
obarczona jest narastającym błędem spowodowanym nawarstwianiem się błędów systematycznych. Efektywne wykorzystanie odometrii wymaga wykonania precyzyjnej kalibracji
[JBF96].
2.2.1
Zasada wykonywania pomiaru
Z silnikami napędzającymi koła połączone są czujniki kąta, takie jak czujniki obrotowo
impulsowe, zwane także enkoderami, lub resolwery. Załóżmy, że promienie obu kół są
jednakowe i równe r, rozstaw między kołami wynosi b, a chwilowe prędkości prawego i
lewego koła wynoszą odpowiednio: ωP i ωL . Równania opisujące prędkość obrotową i
postępową robota w ujęciu wektorowo macierzowym przedstawia poniższa równość:
"
ω
vx
#
"
=
− rb
r
2
r
b
r
2
#"
ωL
ωP
#
,
(2.1)
natomiast pełne równanie stanu w wyrażone w układzie globalnym wygląda następująco:
8
2.3. Rozmieszczenie dalmierza laserowego na robocie

θ̇r


 

 Ẋr  = 
 

Ẏr
2.2.2
r
b
− rb
r
2
r
2
cos θr
sin θr

"

r

cos
θ
r

2
r
sin
θ
r
2
ωL
#
ωP
.
(2.2)
Błędy systematyczne i losowe odometrii
Model odometrii wymaga znajomości dokładnych wartości parametrów geometrycznych
robota. W rzeczywistości informacje te przyjmowane są z pewnym przybliżeniem i po
dłuższym czasie użytkowania urządzenia nawarstwiające się błędy spowodowane tymi rozbieżnościami dają o sobie znać. Ponadto koła mają pewną szerokość i elastyczność, dlatego
kontakt z podłożem nie jest punktowy, jak zakłada przyjmowany model matematyczny. Co
więcej, przekładnie na silnikach zawsze wprowadzają pewne luzy, które również przyczyniają się do powstawania błędów. Nawet jeżeli przeprowadzona została bardzo precyzyjna
kalibracja, po przebyciu odpowiednio długiej trasy rzeczywisty stan robota może znacznie
odbiegać od tego, jaki wskazuje odometria.
Oprócz niedokładności geometrii napędu, w środowisku zaistnieć mogą sytuacje losowe,
które również fałszują odczytywany wynik odometrii. Są to, np. przypadki, w których
koło robota zostanie uniesione w powietrze, skutkiem czego może być krótkotrwała zmiana prędkości (jako wynik zmniejszonego obciążenia). Innym przykładem jest zaistnienie poślizgu poprzecznego lub wzdłużnego. Co więcej, jeżeli rozważamy ruch robota na
płaszczyźnie, wówczas wszelkie jej zniekształcenia również przekłamują wynik estymacji,
ponieważ robot odbywa dodatkową drogę w kierunku pionowym. Mimo tych ograniczeń,
dzięki prostocie realizacji, dużej dokładności krótkoterminowej i dobrej dynamice, odometria jest jednym z podstawowych narzędzi lokalizacji względnej wykorzystywanych w robotach kołowych.
2.3
Rozmieszczenie dalmierza laserowego na robocie
Ponieważ pozycja skanera na przenoszącym go robocie nie jest zwykle jednakowa z
punktem charakterystycznym P , należy wyznaczyć transformację układu związanego z
czujnikiem do układu pojazdu. Zakłada się, że parametry usytuowania sensora względem
układu lokalnego są znane i można je zapisać w postaci ~qs = [ϕs Xs Ys ]T .
Pozycja sensora w globalnym układzie odniesienia jest zależna od stanu robota i
określona wzorami:

ϕgs


θr


1
0
0

ϕs


 
 


 Xgs  =  Xr  +  0 cos θr − sin θr   Xs  ,


 
 

Ygs
Yr
0 sin θr cos θr
Ys
(2.3)
2.3. Rozmieszczenie dalmierza laserowego na robocie
9
Schematycznie rozmieszczenie skanera przedstawiono na rysunku 2.2. Szczegółowy
opis transformacji przedstawiony będzie w rozdziale 3.3.
Rysunek 2.2: Model robota z zamontowanym sensorem.
Rozdział 3
Przegląd sensorów odległości
Sensory odległości to praktyczne narzędzia, w jakie natura wyposażyła kilka gatunków
zwierząt. Robotyka mobilna w dużej mierze opiera na nich swoje działanie, gdyż systemy
wizyjne są na obecnym stanie technologicznym zbyt mało efektywne. Jak wspomniano
we wstępnej części pracy, skanery laserowe w znacznej mierze wyparły inne dalmierze w
zastosowaniu do realizacji problemu SLAM. Mimo to, omówione zostaną także pokrótce
czujniki światła podczerwonego (IR - ang. Infrared ) oraz sonary ultradźwiękowe (US ang. Ultrasonic).
3.1
Czujniki triangulacyjne, wykorzystujące światło
niekoherentne
Dalmierze tego typu charakteryzują się stosunkowo niewielkim zasięgiem pomiarowym.
Współcześnie największą ofertę ma firma Sharp, która produkuje czujniki o zasięgu nawet
do 5,5 metra [sha09]. Zaletą tych czujników jest niewielki rozmiar i pobór prądu (średnio
35 mA przy napięciu 4,5-5,5 V).
3.1.1
Zasada działania
Pojedynczy pomiar polega na emisji skupionej modulowanej wąskiej wiązki światła
z zakresu podczerwieni w kierunku przeszkody i następnym zarejestrowaniu promienia
odbitego przez czujnik PSD (ang. Position Sensitive Detector ) lub matrycę CCD (ang.
Charge Coupled Device). W obu przypadkach do przeliczenia sygnału zwrotnego wykorzystuje się zasadę triangulacji [Paz02]. Odbita wiązka rejestrowana jest przez detektor
pod pewnym kątem ϕ, który w znacznej mierze zależny jest od odległości do przeszkody.
Obrazuje to rys. 3.1.
Zakładając, że przeszkoda znajduje się w skończonej odległości d od urządzenia, zapisać możemy zależność:
tan ϕ =
x
L
= ,
d
D
10
(3.1)
3.1. Czujniki triangulacyjne, wykorzystujące światło niekoherentne
11
Rysunek 3.1: Zasada pomiaru sensoru optycznego.
gdzie L to rozstaw między środkiem emitera i czujnika PSD, D to odległość powierzchni
czujnika PSD od soczewki skupiającej wiązkę, natomiast x to odległość obrazu punktu
P znajdującego się na przetworniku czujnika od osi optycznej. Stąd mierzoną odległość
można obliczyć następująco:
d=
LD
.
x
(3.2)
Wartości L oraz D są znane, gdyż są to parametry konstrukcyjne, natomiast x jest przesunięciem liniowym na powierzchni detektora, które wyznacza się poprzez odpowiednie
przeliczenie zwracanej przezeń wartości prądu.
3.1.2
Charakter błędów i problemy związane z pomiarem
Zwracana przez sensor wartość analogowa ma w stosunku do mierzonej odległości
charakter nieliniowy. W przypadku modelu GP2D12 firmy Sharp niezależne źródła internetowe [Zuf01] opisują znaczne zmiany w precyzji pomiaru w funkcji odległości. Przy
200mm błąd należy do niewielkiego przedziału ±2mm zaś już dla 600mm precyzja ta
spada do ±20mm.
Ponadto, bez odpowiedniej filtracji cyfrowej, wrażliwy na światło zewnętrzne czujnik
PSD oraz prąd pobierany przez emitor LED wprowadzają do pomiaru znaczne szumy na
wyjściu (±100 mV). Sam producent nie zawiera informacji o błędach systematycznych
czujnika w nocie katalogowej.
12
3.2. Sonary ultradźwiękowe
Dopuszczalny kąt padania wiązki podczerwonej ograniczony jest do 40o . Emitowane
światło jest wrażliwe na własności optyczne przeszkody takie jak kolor, gęstość optyczna
czy reflektancja. Modyfikują one intensywność odbitej fali, co przekłada się na wzrost
niepewności pomiarowej.
3.2
Sonary ultradźwiękowe
Ultradźwięki to fale akustyczne o częstotliwości wyższej niż górna granica słyszalności
człowieka (>20kHz, np. 40kHz). Trafiająca na przeszkodę fala implikuje źródło kolejnej
zgodnie z zasadą Huygensa. Wynikiem tego jest, np. odbicie dźwięku czyli tzw. echo.
Właściwość tą wykorzystano w urządzeniach jakimi są sonary. Swoje pierwowzory czerpią
one z natury, gdyż niektóre zwierzęta potrafią w taki sposób lokalizować przeszkody na
swojej drodze.
Zastosowanie znalazły one nie tylko w robotyce, lecz również, a nawet przede wszystkim w marynarce i żegludze. Ponieważ woda ma ponad 780 razy większą gęstość niż
powietrze, fale akustyczne poruszają się w niej około 4,5 razy szybciej. Z tego względu
za pomocą sonarów mierzone mogą być bardzo duże odległości, np. z powierzchni do
dna oceanicznego. Bliskim bratem sonarów są radary, które opierają swoje działanie na
falach radiowych. Sonary ultradźwiękowe zyskały swoją popularność ze względu na niską
cenę, niewielki pobór prądu oraz nieinwazyjność w środowisko (mimo że zaliczamy je do
czujników aktywnych).
3.2.1
Zasada działania
Podstawową metodą pomiaru odległości jest pomiar czasu, jaki fala potrzebuje na
przebycie drogi do przeszkody i z powrotem (TOF - ang. Time Of Flight) [Paz09, Joe09].
W kierunku przeszkody wysyłana jest krótka paczka ultradźwiękowa (ok. 6ms), która po
pewnym czasie odbierana jest przez detektor. Znając prędkość c dźwięku w środowisku
oraz czas propagacji t, możemy określić drogę r z prostej zależności:
r=
ct
.
2
(3.3)
Ponadto wykorzystując znajomość efektu Dopplera, możemy określić zmianę częstotliwości
powracającej fali i na jej podstawie estymować prędkość liniową poruszającego się obiektu.
3.2.2
Charakter błędów i problemy związane z pomiarem
Niestety sonary ultradźwiękowe wykazują sporo właściwości, które znacznie ograniczają
możliwości ich zastosowania. Pierwszą istotną wadą jest szerokość emitowanej fali. W
13
3.2. Sonary ultradźwiękowe
Rysunek 3.2: Zasada pomiaru sonaru US.
zależności od powierzchni przetwornika ρ i długości emitowanej fali λ (rmin =
ρ2
4λ )
wyróż-
niamy, tzw. pole Fresnela (przed rmin ), w którym rozchodzi się ona drastycznie wszerz i
powstają niepożądane listki boczne, oraz tzw. pole Fraunhofera, w którym fala rozchodzi
się w jednym kierunku. Z tego względu pomiar łatwo może być zafałszowany, gdyż odbita
fala może pochodzić ze źródła innego niż przeszkoda (np. z podłoża).
Rysunek 3.3: Fala generowana przez dalmierz ultradźwiękowy.
Ponieważ odbiornik zwykle znajduje się w niewielkiej odległości od emitera, musi być
wyłączony w trakcie generowania fali, gdyż ta dotarłaby do niego przed odbiciem. Powoduje
to istnienie tzw. martwego pola, czyli minimalnej odległości, od której sensor faktycznie
może mierzyć.
Prędkość dźwięku w ośrodkach sprężystych zależna jest od kilku czynników, takich jak
prądy (wiatry), temperatura czy wilgotność. Powoduje to utrudnienia podczas kalibracji
urządzenia. Ponadto jeżeli powierzchnia przeszkody jest gładka i duża w stosunku do
długości fali, może zajść zjawisko odbicia zwierciedlanego, gdy kąt padania będzie zbyt
duży. Niektóre materiały mają właściwości absorbujące silniejsze niż inne, co również
może wpłynąć na precyzję pomiaru. Problem pojawia się także gdy mierzona jest odległość
do narożnika lub zwężenia. Powstające między ścianami echa przekłamują odczytywaną
3.3. Skanery laserowe
14
wartość.
3.3
Skanery laserowe
Dalmierze laserowe to zwykle precyzyjne urządzenia, które do pomiaru odległości
wykorzystują koherentne światło laserowe. Jego zaletami są niewielka długość fali, a
zatem niewielkie zniekształcenia przy różnych właściwościach przeszkody oraz mała szerokość emitowanej wiązki, co pozwala uzyskać dużą rozdzielczość kątową. Jeżeli urządzenie samodzielnie obraca źródło światła i wykonuje serię pomiarów z pewnym krokiem,
mówimy wówczas o skanerach laserowych (również LIDAR - ang. Light Detection and
Ranging).
Rysunek 3.4: Graficzna interpretacja wykonywania skanu.
Początkowo dalmierze laserowe wykorzystywano głównie w geodezji, budownictwie czy
w zastosowaniach militarnych. Skanery laserowe dedykowane były celom przemysłowym,
dotyczącym zagadnień bezpieczeństwa (bariery świetlne), czy kontrola jakości [Com02].
Ich główne zalety, do których należy m.in. szybkość działania i duża precyzja, zostały
dostrzeżone również w obszarze robotyki mobilnej.
W wyniku przeprowadzenia pojedynczego cyklu pomiarowego uzyskuje się N uporządkowanych liczb odpowiadających zmierzonym odległościom. Dla skanerów laserowych najbardziej naturalną reprezentacją wyniku jest reprezentacja polarna (biegunowa). Stąd na
podstawie znajomości kąta emisji wiązki i zmierzonej odległości można określić współrzędne
w globalnym układzie kartezjańskim według następujących zależności:
xi = Xr + Xs cos θr − Ys sin θr + ri cos (θr + ϕs + ϕi )
,
yi = Yr + Xs sin θr + Ys cos θr + ri sin (θr + ϕs + ϕi )
(3.4)
15
gdzie Xr , Yr , θr oznaczają konfigurację robota wyrażoną w globalnym układzie współrzędnych, Xs , Ys , ϕs oznaczają współrzędne i orientację skanera laserowego wyrażone w
lokalnym układzie współrzędnych związanym z robotem, zaś ri i ϕi oznaczają odpowiednio
odległość i kąt emisji wiązki w i-tym pomiarze.
3.3.1
Zasada działania
Pomiar odległości skanerem laserowym polega na pomiarze czasu (TOF, podobnie jak
w sonarach), przesunięcia fazowego ciągłej fali świetlnej o modulowanej sinusoidalnie amplitudzie (tzw. AMCW - ang. Amplitude Modulated Continous Wave) lub na zasadzie
triangulacji, jak w przypadku dalmierzy IR [Skr07].
Metoda TOF została omówiona pokrótce przy opisie sonarów, ale należy nadmienić,
że dla laserów prędkość fali równa jest prędkości światła w ośrodku rozchodzenia się (tu:
powietrze). Oznacza to, że światło przebywa drogę 1m w czasie tT OF = 3,3ns. Dlatego
też stosuje się różne podejścia do problemu zliczania tak niewielkich odcinków czasowych,
np. metody podwójnego noniusza lub ładowanie kondensatora. Aby dokładność pomiaru
była na poziomie centymetrowym, pomiar czasu musi mieć rozdzielczość pikosekundową
[Hol87].
Metoda triangulacji wspomniana była z kolei przy okazji omawiania dalmierzy IR.
Niestety ich sporą wadą jest ograniczony zasięg (zwykle mniej niż 5m) z uwagi na ograniczoną bazę oraz wrażliwość detektora na światło ze środowiska.
Określanie odległości poprzez pomiar przesunięcia fazowego, a więc metoda AMCW,
omija problem techniczny jakim jest pomiar niewielkich odcinków czasu. W przypadku
modulacji o jednej składowej częstotliwości, możemy wyznaczyć odległość ze wzoru:
r=
c
φd ,
4πfAM
(3.5)
gdzie φd oznacza przesunięcie fazowe między falą wysłaną i odebraną, fAM to częstotliwość modulacji amplitudy, zaś c to prędkość światła. Taki typ modulacji stosować można
w ograniczonym zakresie odległości, dla których przesunięcie nie przekracza wartości 2π.
Aby rozszerzyć ten zakres można, np. wprowadzić kolejną składową częstotliwości modulacji i na podstawie dwóch zmierzonych przesunięć fazowych określić faktyczną odległość.
3.3.2
Źródła błędów systematycznych i losowych
W zależności od wybranej metody, wynik pomiaru obarczony jest różnymi błędami
systematycznymi. Dla metody AMCW istotna jest ograniczona dokładność pomiaru fazy
16
zaś dla metody TOF niedokładność pomiaru czasu. Dodatkowa niepewność związana jest
z określeniem kąta wirującego zwierciadła.
Ponieważ skanery są dalmierzami optycznymi, ich praca zakłócana jest przez zmienność parametrów środowiska, do których należy m.in. gęstość optyczna ośrodka propagacji
fali, właściwości reflektancyjne powierzchni przeszkody co może powodować odbicie zwierciedlane padającego promienia lub jego silną absorpcję. Przykładem błędu losowego jest
tzw. efekt śladu optycznego (ang. mixed pixels) wynikający z niezerowej szerokości emitowanego promienia światła. Jeżeli część wiązki trafi na przeszkodę o znacznie innych
własnościach optycznych niż pozostała, informacja niesiona przez powracającą falę jest
zafałszowana. Sytuacja taka może wystąpić, np. gdy wiązka pada na krawędź przeszkody.
3.3.3
Skaner LMS200 firmy SICK
Do przeprowadzonych badań wykorzystano skaner LMS200 firmy Sick AG. Jest to
urządzenie o stosunkowo dużych gabarytach (156 x 155 x 185mm) i wadze (4,5kg), zasilane stałym napięciem 24V i pobierające w trakcie pracy moc bliską 30W.
Z noty katalogowej [LMS06] wynika, że zasięg pomiaru wynosi 8m, przy rozdzielczości
równej 10mm i odchyleniu standardowym na poziomie σ = 5mm. Skanowanie wykonywane jest w różnych trybach, w zakresie: 0o − 180o lub 0o − 100o z rozdzielczością kątową
równą 0,25o , 0,5o lub 1o .
Do pomiaru odległości zastosowano metodę TOF. Urządzenie LMS200 wykorzystuje
światło o długości fali równej 905nm (czyli bliską podczerwień), emitowane przez diodę
laserową o mocy 1mW. Aby pomiar spełniał deklarowaną rozdzielczość 10mm, licznik
czasu musi być taktowany z okresem krótszym niż 66ps. Wiązka światła trafia na wirujące lustro ustawione pod kątem 45o do płaszczyzny pomiaru.
Rysunek 3.5: Uproszczone przedstawienie skanera SICK LMS 200.
Nominalna rozdzielczość skanowania wynosi 1o , co przy prędkości obrotowej lustra ω = 75
obr
s
daje czas pojedynczego pomiaru równy 370µs. Zwiększenie rozdzielczości dwu- lub czterokrotne
wymaga odpowiednio większej ilości wykonanych obrotów lustra.
17
Producent nie po-
daje charakterystyki błędu kątowego wiązki, której orientacja określana jest za pomocą
enkodera. Badania z pracy [Skr07] pozwalają jednak uznać, że błąd ten można opisywać
rozkładem normalnym z odchyleniem standardowym σϕ = 0,083o .
Rozdział 4
Sposoby reprezentacji środowiska
Podjęcie decyzji o wykonaniu ruchu przez robota zwykle opiera się na wiedzy o otaczającym środowisku. Informacja ta zawarta jest w postaci tzw. mapy. Ponieważ rzeczywiste
otoczenie jest zwykle bardzo złożone i często słabo ustrukturyzowane, mapy uwzględniają kluczowe cechy jak usytuowanie przeszkód w przestrzeni. Dla jednostek latających
czy podwodnych opis środowiska przyjmuje formę trójwymiarową. Na potrzeby pracy
omówione zostaną wybrane warianty reprezentacji środowiska 2D. Typowo wyróżnia się
trzy główne sposoby opisu środowiska, do których należą:
• mapa rastrowa,
• mapa wektorowa,
• mapa topologiczna.
Pierwsze dwa wymienione typy należą do kategorii modeli geometrycznych, w których
jawnie przedstawiane są cechy środowiska takie, jakie postrzega człowiek (przeszkody
i wolne przestrzenie). Mapy topologiczne przypisują konkretnym punktom środowiska
odpowiednie identyfikatory i przedstawiają zależności zachodzące między nimi.
4.1
Mapa rastrowa
Generowanie mapy rastrowej polega na zaprojektowaniu siatki o pewnej rozdzielczości,
której pola w trakcie pracy robota sukcesywnie wypełniane są odpowiednimi wartościami
liczbowymi. W najprostszej postaci może być to forma binarna, gdzie 1 oznacza pole
zajęte („ jest przeszkoda”) zaś 0 oznacza pole wolne. W bardziej zaawansowanym przypadku pola przyjmują wartości z przedziału h0,1i, które stanowią o prawdopodobieństwie
zajętości komórki. Modele takie generowane są m.in. przy użyciu reguły Bayesa, teorii
ewidencji Dempstera-Shafera lub w oparciu o zbiory rozmyte [Skr07]. Mapy tego rodzaju
są nieefektywne pod względem zajętości pamięci, lecz są proste w budowie i dają duże
możliwości w procesie generowania ścieżki robota. Teselacja przestrzeni, czyli jej dyskretyzacja, niesie pewną niedokładność i utratę informacji. Przyjmując jednak rozdzielczość
18
19
4.2. Mapa wektorowa
Rysunek 4.1: Przykładowa mapa rastrowa.
siatki małą w stosunku do rozmiarów robota oraz dużą w stosunku do precyzji systemu
pomiarowego, nie stanowi to istotnego kłopotu. W praktyce środowisko dzieli się na
komórki o wielkości z zakresu 1×1cm dla dalmierzy laserowych do 20×20cm dla sonarów
ultradźwiękowych.
Samolokalizacja robota przy wykorzystaniu mapy rastrowej jest utrudniona ze względu
na ogrom zapisanej informacji. Dopasowanie cech można sprowadzić do zadania śledzenia
konturu obiektu znanego z przetwarzania obrazu i widzenia maszynowego.
4.2
Mapa wektorowa
Mapa wektorowa przedstawia obiekty środowiska zewnętrznego za pomocą prymitywnych figur geometrycznych takich jak punkty, odcinki, łuki czy wieloboki, których usytuowanie przedstawione jest jawnie. Ich selekcja na podstawie wykonanego cyklu pomiarowego wymaga stosunkowo długiego czasu przetwarzania, lecz reprezentacja jest efektywna pod względem wykorzystania zasobów pamięciowych systemu. Odnalezione figury i
zależności geometryczne występujące między nimi ułatwiają późniejszy proces dopasowania cech i lokalizacji robota, co doskonale nadaje się do zagadnienia SLAM. Poziom opisu
jest więc bardziej abstrakcyjny niż dla mapy rastrowej.
Problemem map wektorowych jest trudność rejestrowania elementów dynamicznych
środowiska. Zaletą natomiast jest możliwość wykorzystania gotowych map, takich jak
plany budynków, jako wiedzy a priori. Co więcej, każda z uzyskanych cech środowiska
może być reprezentowana poprzez wektor parametrów, będących zmiennymi losowymi.
20
4.3. Mapa topologiczna
Rysunek 4.2: Przykładowa mapa wektorowa.
Ma to istotne znaczenie przy wykorzystaniu narzędzi fuzji danych, przy procesie lokalizacji i mapowania.
W dalszej części pracy zdecydowano się na wykorzystanie mapy wektorowej.
W
głównej mierze decyzja ta motywowana jest precyzją z jaką pozwala ona rejestrować
środowisko oraz możliwościami dopasowania cech, potrzebnych podczas procesu budowy
mapy globalnej i lokalizacji.
4.3
Mapa topologiczna
Rysunek 4.3: Przykładowa mapa topologiczna.
Mapy topologiczne cechują się nieco większym poziomem abstrakcji niż poprzednio
omówione typy. Reprezentują one pewne szczególne usytuowania elementów środowiska,
istotne z punktu widzenia wykonywanego zadania. Określone w ten sposób relacje przed-
4.3. Mapa topologiczna
21
stawiają, np. bezkolizyjną ścieżkę. Najczęściej są one przedstawiane za pomocą grafów,
w których wierzchołki oznaczają lokacje, a krawędzie reprezentują połączenia. Tego typu
mapy zawierają często także cechy geometryczne środowiska tak, aby ich odczyt stawał
się bardziej intuicyjny, jednak nie zawierają szczegółowych kształtów obiektów.
Są to zwykle mapy większych powierzchni, np. pomieszczeń w budynku, których opis
przyjmuje charakter zgrubny. W związku z małą ilością szczegółów, lokalizacja robota
jest znacznie utrudniona i ma szczególne znaczenie na poziomie globalnym.
Rozdział 5
Środowisko symulacyjne procesu
skanowania otoczenia
Ponieważ korzystanie ze sprzętu jakim jest skaner laserowy nie zawsze jest wygodne
i możliwe, na potrzeby pracy opracowane zostało środowisko symulacyjne modelujące
proces pomiaru, umożliwiające wprowadzenie wirtualnego otoczenia jak również dogodne
dobranie parametrów skanera. W wyniku symulacji zwracany jest ciąg liczb, odpowiadający kolejnym odległościom zmierzonym przez wirtualne urządzenie, które poddać można
dowolnej obróbce.
Symulator działa relatywnie szybko i daje praktyczne możliwości wizualizacyjne, które
przydatne są szczególnie podczas analizy algorytmów tworzenia mapy i lokalizacji.
5.1
Model skanera laserowego
Model sensora opisany jest za pomocą grupy zmiennych i parametrów zawierających
takie informacje jak jego pozycja, orientacja, rozdzielczość, ilość pomiarów przypadających
na skan oraz zasięg (także dolną granicę). Ponadto zmierzone odległości oraz kierunek
wirtualnych promieni lasera obarczone są pseudolosowym błędem o rozkładzie normalnym
i zerowej wartości oczekiwanej. Wszystkie te parametry można dowolnie modyfikować.
Przyjęte standardowo wartości wynoszą:
• rozdzielczość: 0,5o ,
• ilość pomiarów N: 361,
• zasięg: 100-8000mm,
• odchylenie standardowe błędu pomiaru odległości: 10mm,
• odchylenie standardowe kąta wiązki: 0,083o .
Wartości ostatnich dwóch spośród wymienionych wzorowano na wnioskach z pracy [Skr07]
dotyczących modelu niepewności skanera LMS200.
22
23
5.2. Model wirtualnego otoczenia
Podczas wizualizacji skaner laserowy przedstawiany jest za pomocą kwadratu o boku
156 jednostek, które utożsamiane są z milimetrami. Zachowana została zatem rzeczywista
wielkość urządzenia. Pozycja w układzie globalnym określana jest względem środka figury.
Wychodzący z niego odcinek obrazuje orientację skanera.
5.2
Model wirtualnego otoczenia
Założono, że przeszkody i granice wirtualnego otoczenia budowane są za pomocą odcinków, jako że są to figury geometryczne łatwe w opisie i analizie. Świat rzeczywisty jest
skomplikowany geometrycznie, a przedstawianie go za pomocą skończonej ilości odcinków
stanowi olbrzymie uproszczenie.
Jednak dla wielu rzeczywistych środowisk o dobrze
uwarunkowanej strukturze wybrana reprezentacja jest wystarczająca. Zaprojektowanie
nowej przeszkody polega na wprowadzeniu współrzędnych kartezjańskich początku i końca
odcinka do odpowiedniej tablicy. Przykładowe otoczenie przedstawiono na rysunku 5.1.
Rysunek 5.1: Przykładowe zamodelowane otoczenie.
Podając współrzędne odcinków, należy mieć na uwadze, że wirtualne jednostki odpowiadają
milimetrom. Praktyczną uwagą podczas modelowania otoczenia jest wykorzystanie pojedynczego odcinka o dłuższych wymiarach zamiast dwóch, które leżą na wspólnej prostej.
Ogranicza to w dalszej części ilość obliczeń, a nie zmienia w żaden sposób rezultatów
pomiaru.
5.3
Proces pomiarowy
Wykonanie sekwencji pomiarowej wymaga podania informacji o otoczeniu i konfiguracji lasera. W pierwszej kolejności generowany jest wektor parametrów N prostych,
reprezentujących wiązki laserowe wychodzące z urządzenia. Przechodzą one przez środek
skanera i kolejne punkty na okręgu jednostkowym wokół niego. Na tym poziomie uwzględniany jest model niepewności odchylenia wiązki. Parametry prostych dla kolejnych wiązek
24
5.3. Proces pomiarowy
generowane są w kierunku przeciwnym do wskazówek zegara, czyli, z punktu widzenia
skanera, od prawej do lewej. Przyjęto konwencję zapisu wykorzystanych figur w postaci
ogólnego równania prostej Ax + By + C = 0. W ten sposób można uniknąć problemu
osobliwości (który napotyka równanie kierunkowe dla odcinków pionowych) i otrzymać
pewne rozwiązanie punktu przecięcia z inną prostą. Podobne parametry (A, B, C) generowane są dla każdej z przeszkód, tworząc drugi wektor prostych.
Rysunek 5.2: Przykładowy skan wykonany w wirtualnym pomieszczeniu.
Poniżej przedstawiono uproszczony zapis algorytmu wykonywania pomiaru.
Algorytm 1: Pomiar
1
for i =1:1: IloscPomiarow
2
r ( i ) = 10000;
3
[Am Bm Cm] = ParametryWiazki ( Xs , Ys , s i g m a p h i , i ) ;
4
for j =1:1: IloscOdcinkow
5
[ Ao Bo Co ] = ParametryPrzeszkody ( j ) ;
6
[X Y] = WyznaczPunktPrzeciecia (Am,Bm,Cm, Ao , Bo , Co ) ;
7
d = WyznaczOdleglosc (X, Y, Xs , Ys ) ;
8
i f (d < r ( i ))
9
r( i ) = d;
end
10
11
end
12
i f ( r ( i ) > 8000 | | r ( i ) < 1 0 0 )
13
14
r ( i ) = 0;
else
r ( i ) = r ( i ) + Blad ( s i g m a r )
15
16
end
17 end
25
5.3. Proces pomiarowy
Algorytm wykorzystuje podwójną pętlę for. Pierwsza z nich przeprowadza iterację
po wektorze wiązek laserowych, druga po wektorze przeszkód. Dla każdej wiązki, po
kolei odnajdywane są punkty przecięcia ze wszystkimi przeszkodami przy jednoczesnym
sprawdzaniu czy odnaleziony punkt faktycznie należy do aktualnie analizowanej figury (czy
nie znajduje się poza odcinkiem). Punkt przecięcia dwóch prostych A1 x + B1 y + C1 = 0
i A2 x + B2 y + C2 = 0 wyznacza się wykorzystując teorię wyznaczników i wzory Cramera:
"
A1 B1
A2 B2
#"
x
y
#
"
=
−C1
−C2
#
"
⇒
x
y
#
"
=
A1 B 1
A2 B 2
#−1 "
−C1
−C2
#
.
(5.1)
Spośród znalezionych punktów dla danej wiązki wybierany jest ten o najmniejszej
odległości do skanera. Na koniec dodawany jest błąd pomiarowy zgodny z modelem
niepewności.
Algorytm wykonuje zatem O(n·m) obliczeń, gdzie n oznacza ilość pomiarów przypadających na skan, zaś m ilość zaprojektowanych przeszkód. Przykładowy wynik takiej operacji obrazuje rysunek 5.2. Skaner zaznaczony jest za pomocą kwadratu i odcinka wskazującego jego orientację. Czarne punkty przedstawiają pomiary wykonywane z ruchem przeciwnym do wskazówek zegara.
Rozdział 6
Ekstrakcja cech geometrycznych z
pojedynczego skanu
W wyniku przeprowadzenia pojedynczego cyklu pomiarowego otrzymywany jest szereg kolejnych zmierzonych odległości. Bez odpowiedniej analizy są to jedynie mało istotne liczby, które bezpośrednio mogą znaleźć zastosowanie tylko dla niewielkiej klasy
algorytmów dotyczących sterowania robotów mobilnych. Proces odzyskiwania informacji o zbadanym otoczeniu, nazywany często ekstrakcją cech geometrycznych, polega na
odnajdywaniu grup punktów, które mogą reprezentować pewną przeszkodę (landmark) i
zarejestrowaniu ich w prostszej postaci, np. jako odcinek czy łuk. Figury te nazywane
będą dalej prymitywami geometrycznymi lub po prostu prymitywami.
Zadaniem ekstrakcji jest nie tylko ostateczne wydzielenie potencjalnych przeszkód,
lecz również wstępna eliminacja błędnych pomiarów, powstałych, np. wskutek śladu optycznego czy innych niedokładności narzędzia. Występowanie takich błędów nie zostało
uwzględnione w środowisku symulacyjnym, jednak należy mieć je na uwadze podczas projektowania algorytmu do użycia w środowisko naturalnym.
6.1
Grupowanie
Pierwszym krokiem jest określenie przynależności poszczególnych elementów uzyskanych
na podstawie pomiarów do zbiorów, na podstawie których w późniejszej fazie wyłaniane
będą prymitywy. Jednym ze sposobów klasyfikowania punktów do grup jest badanie
kryterium odległości euklidesowej pomiędzy dwoma kolejnymi zmierzonymi wartościami.
Wymaga to wyrażenia współrzędnych tych punktów we współrzędnych kartezjańskich i
następnie obliczeniu przesunięcia między nimi, poprzez wykorzystanie nieliniowej operacji
pierwiastkowania. Można tutaj zauważyć, że im dalej przeszkoda znajduje się od skanera,
tym mniejsze jest zagęszczenie znalezionych na niej punktów (rys. 6.1). Oznacza to, że kryterium klasyfikacji powinno zmieniać się elastycznie w stosunku do mierzonej odległości.
26
27
6.1. Grupowanie
Biorąc pod uwagę efektywność obliczeniową oraz fakt, że „naturalnym” zapisem pomiarów dla skanerów laserowych jest postać biegunowa, zdecydowano się na wykorzystanie
procedury grupowania zaproponowanej w pracy [Skr07]. W metodzie tej zamiast oblicza-
Rysunek 6.1: Ilustracja zmniejszania się zagęszczenia pomiarów wraz z malejącym
kątem padania promieni do płaszczyzny przeszkody.
nia odległości euklidesowej między dwoma punktami badany jest stosunek wartości dwóch
następujących po sobie pomiarów. Punkt następny klasyfikowany jest do tej samej grupy
co poprzedni, jeżeli stosunek ten znajduje się w pewnym ustalonym przedziale. W przeciwnym wypadku rozpoczyna on nową grupę. W tym celu definiuje się dwa klasyfikatory
kmin i kmax , które określają dolną i górną granicę tego przedziału.
Przykładowymi wartościami tych parametrów mogą być kmin = 0,9 i kmax = 1,1, co
w praktyce oznacza, że pomiar następny przypisany zostanie do tej samej grupy wtedy i
tylko wtedy, gdy jego wartość nie przekracza 90% wartości pomiaru poprzedniego w dół i
110% wartości pomiaru poprzedniego w górę.
Jest to metoda wymagająca bardzo niewielkich nakładów obliczeniowych (O(n)). Przykładowy zapis tego algorytmu w pseudokodzie wygląda następująco:
Algorytm 2: Grupowanie
1 GroupNumber = 1 ;
2 NumberOfGroup ( 1 ) = GroupNumber ;
3
4
5
6
f o r i = 1 : 1 :N−1
i f ( r ( i +1) >= r ( i ) ∗Kmin && r ( i +1) <= r ( i ) ∗Kmax)
NumberOfGroup ( i +1) = GroupNumber ;
else
7
GroupNumber = GroupNumber + 1 ;
8
NumberOfGroup ( i +1) = GroupNumber ;
9
end
6.2. Segmentacja
28
10 end
gdzie N oznacza ilość pomiarów, a r(i) pomiar obecny. W wyniku tego procesu otrzymuje
się wektor liczb całkowitych o rozmiarze 1×N, którego wartości oznaczają numer grupy,
do której należy dany pomiar oraz skalar zawierający liczbę uzyskanych grup. Im bardziej
zawężony zostanie przedział klasyfikacji, tym więcej z jednego skanu uzyska się grup.
Problemem grupowania są przeszkody mało odległe od robota, których wektor normalny zorientowany jest do padających promieni lasera prawie prostopadle. Oznacza
to, że mimo bliskiego występowania po ich powierzchni „rozstrzelonych” zostanie jedynie
kilka pomiarów, które z dużym prawdopodobieństwem zostaną sklasyfikowane jako osobne
grupy, eliminowane w późniejszym etapie. Inną kwestią są pojedyncze pomiary na tle większej przeszkody, które rozbijają ją na dwie niezależne grupy. Oba problemy zobrazowano
na rysunku 6.2. Grupy będące wynikiem pojedynczego pomiaru są kłopotliwe w inter-
Rysunek 6.2: Problem odszukiwania przeszkody o znacznym kącie nachylenia (po
lewej) i problem pojedynczych pomiarów rozbijających większe grupy.
pretacji. Niosą one pewną informację, która może okazać się istotna (np. gdyby promień
lasera odbiła bardzo wąska, lecz solidna przeszkoda znajdująca się przed robotem), aczkolwiek trudno rozpatrywać pojedynczy punkt jako istotny na tle kompletnej mapy otoczenia.
Algorytm eliminuje takie grupy, gdyż na poziomie laboratoryjnym zakłada się ruch jednostki w środowisku silnie ustrukturyzowanym, pozbawionym tego typu niedogodności.
6.2
Segmentacja
Ponieważ w każdej z wyróżnionych grup może znajdować się obiekt nie dający się
opisać za pomocą jednego prymitywu, każdą z nich można ponownie przeanalizować w
poszukiwaniu podgrup, które można przybliżyć, np. odcinkiem. Taki proces nazywany
jest w literaturze segmentacją. Istnieją różne podejścia do tego zagadnienia, opierające
swoje działanie, np. na badaniu kolejnych odległości, algorytmie RANSAC [RB09] czy
transformacie Hougha (znanej m.in. z przetwarzania obrazu).
29
6.2. Segmentacja
Wybraną metodą segmentującą pogrupowany wcześniej skan jest algorytm iteracyjnego
poszukiwania punktów końcowych (ang. Iterative End Point Fit). Wykorzystuje on
reprezentację kartezjańską wyników pomiarów i polega na progresywnym odnajdywaniu
tych, które reprezentują wierzchołki figury jak również łączeniu ich za pomocą odcinków.
W początkowym kroku na pierwszym i ostatnim punkcie grupy oparta zostaje pomocnicza
prosta, dla której wyznaczone zostają parametry A, B oraz C. Następnie pośród całej
grupy poszukiwany jest punkt najbardziej od niej odległy. Jeżeli odległość ta jest mniejsza
od zadanego minimum dmin , wówczas algorytm kończy działanie, zwracając parametry
prostej opartej na krańcowych punktach zbioru jako dostateczny opis grupy. W przeciwnym wypadku odcinek zostaje podzielony w znalezionym punkcie na dwa odcinki, które
należy przeanalizować w ten sam sposób. Algorytm działa do chwili, w której odnalezione
Rysunek 6.3: Segmentacja przykładowego landmarku.
zostaną wszystkie odcinki nie dające się dalej łamać (dla których wszystkie punkty przynależące spełniają wartość d < dmin ). Proces segmentacji przykładowego obiektu zobrazowano na rysunku 6.3. Wartość dmin musi być na tyle wysoka, aby odfiltrować szum
pomiarowy i na tyle niska aby rozróżniać faktyczne kształty otoczenia. Zakładając odchylenie standardowe pomiaru równe 10mm, można zgodnie z regułą 3σ przyjąć dolny
próg dmin jako 30mm. Ponieważ jednak wymiary skanera SICK, a tym bardziej robota,
który go niesie, są znacznie większe od tej wartości, praktyczna wartość dmin w mieści się w
zakresie 50-80mm. Im mniejsza wartość dmin , tym większe staje się prawdopodobieństwo
wyróżnienia dodatkowych prymitywów, a co się z tym wiąże, wzrost czasu przetwarzania
danych.
30
6.3. Analiza statystyczna odcinków
Realizacja algorytmu nie jest zadaniem prostym.
Poniżej przedstawiono możliwe
rozwiązanie problemu.
Algorytm 3: Segmentacja
1
IloscPrymitywow = 0 ;
2 KoniecGrupy = 1 ;
3 while ( KoniecGrupy < N)
4
IloscPrymitywow = IloscPrymitywow + 1 ;
5
while ( t r u e )
6
7
ZliczPunktyWGrupie ;
8
OznaczPoczatekIKoniecGrupy ;
9
i f ( IlePunktowWGrupie > 2 )
10
11
ZnajdzMaxOdlegloscOdPunktu ;
12
OznaczPunktPrzelamania ;
else
13
14
MaxOdleglosc = 0 ;
15
end
16
i f ( IlePunktowWGrupie > 1 && MaxOdleglosc > Dmin)
17
ModyfikujNumeryGrup ;
else
18
break ;
19
end
20
end
21
22 end
Nadrzędna pętla algorytmu wykonywana jest dopóki analizie nie zostanie poddana
ostatnia grupa, co implikuje indeks KoniecGrupy = N, gdzie N jest ilością pomiarów w
skanie. Podrzędna pętla dokonuje analizy pojedynczej grupy. Jeżeli największa znaleziona
odległość przewyższa wartość dmin , indeksy grup wszystkich następnych punktów zwiększają się o jeden i analizie poddawana jest kolejna grupa.
6.3
Analiza statystyczna odcinków
Wyznaczone parametry odcinków oszacowane zostały na podstawie jedynie dwóch
punktów krańcowych, które niekoniecznie dobrze odzwierciedlają faktyczne usytuowanie
przeszkody w środowisku roboczym. Z tego powodu należy uśrednić otrzymane parametry,
uwzględniając wszystkie punkty wchodzące w skład podgrupy. Ponadto, ze względu na
późniejsze kroki dotyczące budowy mapy otoczenia, zdecydowano się na zmianę konwencji
31
6.3. Analiza statystyczna odcinków
zapisu parametrów i wykorzystania równania prostej postaci normalnej
x cos ϕ + y sin ϕ + ρ = 0,
(6.1)
gdzie ϕ ∈ h0,2π) oraz ρ > 0. Reprezentacja ta została zobrazowana na rysunku 6.4. Forma
ta do obliczenia parametru ϕ wymaga silnie nieliniowej funkcji arctan, jednak zapis redukuje się z trzech do dwóch parametrów. Wzory opisujące oba parametry wyrażają się
następująco:
1
ϕ = arctan
2
−2
Pnp
Pnp
− yi )(x̄ − xi )
2
2
i=1 [(ȳ − yi ) − (x̄ − xi ) ]
i=1 (ȳ
,
ρ = x̄ cos ϕ + ȳ sin ϕ,
(6.2)
(6.3)
gdzie x̄ i ȳ to odpowiednie wartości średnie (wartości oczekiwane) parametru w grupie. Do
obliczeń należy wykorzystać dwuargumentową funkcję atan2, której wynik przekształca
się następnie do zakresu ϕ ∈ h0,2π).
Prosta to figura geometryczna o nieskończonej długości. Aby określić usytuowanie
Rysunek 6.4: Reprezentacja linii w postaci normalnej.
leżącego na niej odcinka należy podać dodatkowe informacje. Mogą być to, np. współrzędne
krańcowych punktów odpowiednio rzutowanych na tę prostą, co razem z ϕ i ρ daje sześć
wartości skalarnych potrzebnych do zapisu prymitywu. Innym sposobem jest wyznaczenie
środka i długości odcinka. Taki zapis redukuje ilość zmiennych do pięciu, lecz wymaga
dodatkowych nakładów obliczeniowych. Niemniej jednak wartym uwagi jest parametr długości, który ma znaczenie podczas procesu dopasowania mapy lokalnej i globalnej. Ostateczny zapis pojedynczego prymitywu wyłuskanego ze skanu zawiera wszystkie wymienione
parametry i ma następującą postać:
w
~ = [ϕ ρ Pp Pk Pc L] ,
6.4. Rekonstrukcja prymitywów na poziomie analizy skanu
32
gdzie Pp Pk Pc L to odpowiednio współrzędne początku, końca, środka odcinka i jego
długość.
Rzutowanie krańcowych punktów na skorygowaną prostą polega na wyznaczeniu parametrów
prostej prostopadłej przechodzącej przez owy punkt i wyznaczeniu miejsca przecięcia obu
figur. Ponieważ równanie normalne jest jedynie znormalizowaną postacią równania ogólnego, można posiłkować się w tym przypadku liniowym rozwiązaniem zadania za pomocą
wyznaczników.
6.4
Rekonstrukcja prymitywów na poziomie analizy skanu
Często otrzymane z procesu segmentacji prymitywy obarczone są niedokładnościami, które niosą redundantną lub niepełną informację. Są to z reguły śladowe odchylenia współrzędnych lub zniekształcenia geometryczne, których eliminacja może jednak
znacząco wspomóc i zwiększyć pewność dalszego procesu mapowania. Różne działania
dotyczące rekonstrukcji odcinków opartych na serii punktów opisano w pracy [VV04].
Poniżej przedstawiono zastosowanie dwóch operacji.
6.4.1
Łatanie dziur
Wspomniany w sekcji 6.1 problem rozbicia większej grupy przez pojedynczy pomiar
daje w wyniku dwa zamiast jednego prymitywu. Późniejsze dopasowanie na mapie globalnej wymaga w takim przypadku dodatkowych obliczeń. Warto zatem jeszcze na poziomie
przetwarzania pojedynczego skanu dokonać próby eliminacji tej niedogodności. Operatorem jaki tutaj zastosowano jest „łatanie dziur”.
Jeżeli dwa prymitywy cechują się zbliżoną wartością parametrów ϕ i ρ (warunki
spójności fazy i odległości) oraz punkt początkowy jednego i końcowy drugiego znajdują się w bliskim sąsiedztwie, wówczas uznaje się oba odcinki za część jednego większego
obiektu i dokonuje się ich spojenia.
Należy zatem wyznaczyć trzy klasyfikatory δmin , ϕmax i ρmax , określające przedziały
dozwolonych odchyleń parametrów. Przykładowe wartości to δmin = dmin (patrz: sekcja
6.2), ϕmax = 10o i ρmax = 200mm (w przypadku dużych map niewielka niedokładność
parametru ϕ może spowodować znaczącą zmianę parametru ρ). Zostały one dobrane
w sposób empiryczny. Podczas implementacji warunku fazy należy mieć na uwadze okresowość miary kątowej lub jej nieciągłość (w zależności od przyjętej konwencji). Rozwiązanie
tego problemu zamieszczono w dodatku A.
Gdy dwa odcinki spełniają wszystkie wymienione warunki, należy określić prostą
wspierającą, na której leżeć będzie wynikowy prymityw. Nowe parametry ϕ i ρ sza-
33
Rysunek 6.5: Poprawa jakości uzyskanych odcinków po zastosowaniu uśredniania
statystycznego i łatania dziur.
cowane są jako średnia ważona wartości wcześniejszych figur, przy czym za wagę uznajemy parametr długości poszczególnych z nich (przyjmując na wiarę, że dłuższy odcinek
jest lepszym reprezentantem). Lepszym wskaźnikiem wagowym byłaby odwrotność wariancji punktów wokół odcinka. Współrzędne punktów krańcowych są wynikiem rzutowania
odpowiednich, skrajnych punktów poprzednich odcinków na nową prostą. Środek i długość nowego prymitywu obliczane są na ich podstawie.
Test odległości można wykonać także poprzez obliczenie odsunięcia punktów krańcowych jednego prymitywu od prostej wspierającej drugiego (rys. 6.6) i sprawdzenie,
czy suma otrzymanych wartości nie przekracza określonego minimum. Jest to bardziej
czasochłonne, jednak powinno stosowane być w przypadku analizy krótkich odcinków.
Uzasadnione jest to faktem, iż niewielka różnica kąta (nadal spełniająca warunek fazy)
może nieść za sobą spore odchylenia wartości ρ.
6.4.2
Poprawa i rejestracja narożników
W wyniku segmentacji każda grupa wyłania pewną ilość prymitywów. Jeżeli jest ona
większa niż jeden, oznacza to, że nastąpiło przełamanie podstawowej prostej wspierającej, co wiąże się z istnieniem narożnika. Jest on punktem wspólnym obu tworzących
go odcinków. Po uśrednieniu prostych na podstawie wszystkich punktów należących do
grupy okazuje się, że proste opisujące oba te odcinki uległy odchyleniom, a informacja o
narożniku została utracona.
34
Rysunek 6.6: Badanie warunku odległości za pomocą punktów krańcowych.
Jeżeli dowolna para punktów krańcowych dwóch odcinków znajduje się w bliskim
otoczeniu, a warunek spójności fazy między nimi nie jest spełniony, oznacza to, że pretendują one do rekonstrukcji narożnika. Przypadek taki zobrazowano na rysunku 6.7.
Rysunek 6.7: Od lewej: wycinek mapy globalnej, niepoprawny narożnik, narożnik
poprawiony.
W celu dokonania rekonstrukcji wierzchołka należy odnaleźć punkt przecięcia obu
prostych i wstawić otrzymane współrzędne w miejsce odpowiednich punktów krańcowych
odcinków. Po dokonaniu tej zmiany wymagane jest wprowadzenie zmian parametrów
długości oraz środka odcinka. Jak widać z rysunku 6.7 poprawa jest niewielka, jednak
odnalezienie informacji o narożniku może okazać się bardzo praktyczne. Należy mieć
na uwadze, aby do poprawy narożników nie uwzględniać stosunkowo krótkich odcinków
(< 2dmin ). Taka zmiana może okazać się w ich przypadku diametralna.
W celu zwiększenia precyzji procesu dopasowania cech geometrycznych, mapę otoczenia,
składającą się dotychczas jedynie z odcinków, warto uzupełnić informacją o narożnikach
jako nowego rodzaju prymitywem geometrycznym. Struktura zawierająca narożnik powinna
nieść takie informacje jak jego współrzędne, kąt rozwarcia, orientację oraz ewentualnie
wskaźniki na tworzące go odcinki. Wartości kąta rozwarcia oraz orientacji nie zawsze
są proste w ustaleniu, ponieważ ciężko jest określić warunek wklęsłości lub wypukłości
narożnika. Nie jest bowiem jednoznaczne która ze stron dwóch półprostych o wspólnym
35
punkcie początkowym jest „aktywna”, a która „pasywna”, tzn. z której strony znajduje
się przestrzeń wolna, a z której przeszkoda.
Analiza skanu daje w wyniku mapę lokalną uporządkowaną, tzn. której elementy pon-
Rysunek 6.8:
narożników.
Schematyczne przedstawienie opisu dwóch przykładowych
umerowane są w kolejności występowania (od prawej do lewej, czyli tak jak początkowo
grupy). Ponadto znane są kąty ϕi prostych wspierających. Możemy kąt rozwarcia określić
zatem za pomocą wzoru:
180o − (ϕi+1 − ϕi ),
(6.4)
gdzie różnica zawarta w nawiasie dopuszcza wyniki o wartościach ujemnych. Wartość 180o
zapewnia wówczas sprowadzenie wyniku do przedziału h0,2π), z którego w sposób jasny
wynika informacja o wklęsłości lub wypukłości wierzchołka.
Orientacja γ narożnika powinna być określona na podstawie dwusiecznej jego kąta.
Może być to osiągnięte poprzez dodanie połowy kąta rozwarcia do wartości ϕi . Ponieważ
skaner działa w zakresie 0o − 180o , dla mapy lokalnej γ może zawierać się jedynie w
przedziale (π,2π), gdyż wskazy znalezionych mogą skierowane być jedynie do środka, a
więc w stronę skanera. Jednak po rzutowaniu na mapę globalną przedział ten rozszerza
się do pełnego okresu.
W procesie lokalizacji globalnej, czyli zgrubnego odnalezienia robota w środowisku na
podstawie pojedynczego skanu, największą informację niosą znalezione prymitywy, które
są w pełni widzialne. Znamy wówczas dokładnie ich parametr długości, za pomocą którego
możemy podjąć pierwszą próbę dopasowania. Niestety nie zawsze łatwo jest ocenić czy
odcinek był w pełni widzialny czy też nie. W przypadku narożników problem ten nie
istnieje.
Rozdział 7
Budowa mapy globalnej przy
pełnej znajomości konfiguracji
robota
Rozważmy idealny przypadek, w którym konfiguracja robota jest znana dla każdej
chwili t. Problem lokalizacji jest zatem rozwiązany. Pozostaje do wykonania proces
budowy mapy otoczenia. Określenie usytuowania znalezionych prymitywów względem
układu podstawowego sprowadza się do prostych przekształceń geometrycznych. Problem
stanowi jednak aktualizowanie mapy na zasadzie łączenia tych odcinków, które reprezentują wspólny landmark.
Jeżeli przykładowo połowa ściany wykryta zostanie w jednym cyklu pomiarowym, zaś
w następnym widoczna będzie już całość, należy te dwa prymitywy scalić, gdyż przedstawiają jedną cechę środowiska. Istotą tego procesu jest zatem m.in. minimalizowanie
wielkości pamięci, jaką zabiera mapa, poprzez minimalizowanie liczby prymitywów.
Proces budowy mapy otoczenia ma charakter o tyle praktyczny, że jego wynik może
być wykorzystany przez inne platformy do późniejszych zadań wykonywanych na tym
samym terenie.
7.1
Rzutowanie uzyskanych prymitywów
Za początek układu współrzędnych związanego z mapą przyjmuje się w miejsce wykonania pierwszego skanu, a zatem punkt X0 , Y0 . Dodatkowo mapa obrócona jest względem
układu podstawowego o kąt θ0 . Każdy kolejny cykl pomiarowy daje w wyniku odcinki,
które należy analizować po wcześniejszym rzutowaniu do tego układu współrzędnych. Zapiszmy trzy różnice, istotne podczas późniejszych działań:
36
7.2. Analiza podobieństw między prymitywami
37
∆θ = θr − θ0 ,
∆X = (Xr − X0 ) cos θ0 − (Yr − Y0 ) sin θ0 ,
(7.1)
∆Y = (Xr − X0 ) sin θ0 + (Yr − Y0 ) cos θ0 .
Należy pamiętać, że wzory te słuszne są przy założeniu prawoskrętnego układu współrzęd-
Rysunek 7.1: Zobrazowanie procesu rzutowania do układu związanego z mapą
przy znanej konfiguracji w układzie podstawowym.
nych i robocie zwróconym zgodnie z osią OX układu podstawowego dla θr = 0. Parametry
prymitywów wyznaczonych w procesie ekstrakcji przekształca się do układu związanego
z mapą korzystając z prostych zależności geometrycznych. Nowe wartości parametrów
otrzymujemy na podstawie następujących równań:
ϕm = ϕ + ∆θ,
ρm = ρ + ∆X cos ϕm + ∆Y sin ϕm ,
(7.2)
Pxm = ∆X + Px cos ∆θ − Py sin ∆θ,
Pym = ∆Y + Py cos ∆θ + Px sin ∆θ,
gdzie Px , Py są współrzędnymi parametrów typu punkt. Parametr długości jest wobec
zastosowanych transformacji geometrycznych niezmienniczy.
7.2
Analiza podobieństw między prymitywami
Przekształcone według zależności 7.2 prymitywy należy porównać z tymi już istniejącymi na mapie w celu znalezienia cech wspólnych. W pierwszej kolejności badane są
38
7.2. Analiza podobieństw między prymitywami
zachodzące między nimi warunki spójności fazy i odległości, omówione w punkcie 6.4 przy
okazji omawiania rekonstrukcji na poziomie analizy skanu. Są to warunki konieczne, lecz
nie dostateczne spojenia dwóch prymitywów. Podobne parametry mogą mieć dwa lub
więcej odcinków, które reprezentują zupełnie odmienne landmarki. Problem ten zobrazowano na rysunku 7.2. Z tego względu wprowadza się warunek zachodzenia na siebie
Rysunek 7.2: Nieprawidłowe scalenie dwóch prymitywów przy spełnionych
warunkach fazy i odległości. Od lewej: wykonany skan, zbudowana na jego podstawie mapa oraz mapa zaktualizowana po wykonaniu drugiego skanu.
odcinków [Skr07]. Polega on na zbadaniu wszystkich możliwych odległości występujących
między punktami krańcowymi odcinków (rys. 7.3) i sprawdzeniu zależności:
D1 + D2 + D3 + D4 < 2X + 2Y + 2dmin .
Rysunek 7.3: Test zachodzenia na siebie prymitywów.
Jeżeli odnalezione zostały dwa prymitywy spełniające te trzy założenia, należy wyznaczyć na ich podstawie parametry nowego odcinka. Przebiega to w sposób następujący:
1. Obliczamy nowy parametr ϕm jako średnią ważoną ϕ1 i ϕ2 , gdzie za wagę przyjmujemy parametry długości L1 i L2 :
ϕm =
ϕ1 L1 + ϕ2 L2
.
L1 + L2
(7.3)
39
7.3. Rekonstrukcja mapy globalnej
2. Obliczamy w podobny sposób parametr ρm :
ρm =
ρ1 L1 + ρ2 L2
.
L1 + L2
(7.4)
3. Szukamy największej odległości spośród D1, D2, D3, D4, X i Y (rys. 7.3). Na nową
prostą o parametrach ϕm , ρm rzutujemy punkty Pp i Pk , które tworzą najdłuższy
odcinek.
4. Obliczamy współrzędne środka i nową długość Lm .
Każdy element mapy zapamiętuje liczbę aktualizacji nu , które odbyły się z jego udziałem. Jest to pewnego rodzaju wyznacznik niepewności prymitywu.
Algorytm 4 przedstawia proces dopasowania cech mapy lokalnej do mapy globalnej.
Algorytm 4: DopasowanieCech
f o r i = 1 : 1 : IloscPrymitywowLokalnych
1
f o r j = 1 : 1 : IloscPrymitywowGlobalnych
2
3
W1 = WarunekFazy ( i , j ) ;
4
W2 = WarunekOdleglosci ( i , j ) ;
5
W3 = WarunekZachodzenia ( i , j ) ;
6
i f (W1 && W2 && W3)
7
8
ZaktualizujOdcinek ( j ) ;
else
9
10
DodajNowyOdcinek ;
end
11
end
12
13 end
Badania symulacyjne pokazały, że czas liczony od uzyskania danych ze skanera do
zakończenia aktualizowania mapy, która zawiera już 60 obiektów wynosi średnio 32ms.
7.3
Rekonstrukcja mapy globalnej
Aktualizacja mapy wprowadza do niej nowe odcinki lub modyfikuje parametry poprzednich. Wiąże się to z możliwością wystąpienia pewnych niedokładności. Należy wyeliminować je poprzez rekonstrukcję geometryczną. Proces ten nie musi być wykonywany wraz
z każdą aktualizacją, lecz przykładowo co dziesiąty krok. Polega na zbadaniu relacji geometrycznych pomiędzy każdą parą różnych odcinków.
W ogólności sam proces aktualizacji powinien radzić sobie z odcinkami zachodzącymi na siebie. Ponieważ jednak mogą wystąpić przypadki szczególne, warto sprawdzić,
7.4. Wyniki eksperymentalne
40
Rysunek 7.4: Porównanie wycinka mapy bez (po lewej) i po rekonstrukcji.
czy analizowane prymitywy nie pretendują do tego typu korekcji. Pozostałymi operatorami jakie można wykorzystać w celu korekcji mapy są „łatanie dziur” oraz poprawa
narożników. Omówiono je w rozdziale 6.4. W przypadku pierwszych dwóch, należy pamiętać, że z pary prymitywów otrzymujemy w wyniku tylko jeden. Dane o drugim należy
stosownie zwolnić lub wyzerować. Co więcej, należy zachować przedstawioną powyżej
kolejność sprawdzania warunków dla pary odcinków. Najpierw badana jest możliwość
zespolenia (operacja najwyższej rangi), następnie załatania dziury, a dopiero na końcu
poprawy narożników (operacja najniższej rangi).
W pierwszej fazie mapowania warto wykonać kilka cyklów pomiarowych bez zmiany
stanu robota. Jeżeli po, przykładowo 10-krotnej iteracji w strukturze mapy znajdują się
elementy o wartości nu równej 1, należy usunąć je zakładając, że są to obiekty przypadkowe lub dynamiczne. W trakcie ruchu robota również można regularnie sprawdzać ten
warunek. Nawet jeżeli usunięty zostanie odcinek reprezentujący rzeczywisty obiekt, w
dalszym procesie zostanie on ponownie odnaleziony i wprowadzony do struktury mapy.
Między innymi z tego powodu nie należy dokonywać rekonstrukcji po każdym wykonanym
cyklu pomiarowym.
7.4
Wyniki eksperymentalne
W celu sprawdzenia poprawności działania algorytmu przeprowadzone zostały odpowiednie badania. Ostateczne rezultaty przedstawiono po wielokrotnym ponawianiu testów
i każdorazowym dostrajaniu przedziałów ufności. Nie oznacza to, że dobrane zostały
one w sposób optymalny. Dobór tych nastaw jest silnie uzależniony jest od charakteru
środowiska.
7.4.1
Badania symulacyjne
Wykorzystując prezentowane w rozdziale 5 wirtualne otoczenie (rys. 5.1) wykonano
symulację składającą się z 52 pomiarów wykonywanych w różnych jego punktach. Rekon-
41
strukcja mapy dokonywana była co ósmy pomiar. Na końcu dokonano dodatkowej rekonstrukcji i usunięto elementy o wartości nu równej 1. Wynik eksperymentu przedstawiono
na rysunku 7.5.
Rysunek 7.5: Mapa otoczenia powstała w wyniku symulacji.
Otrzymana mapa w ogólności poprawnie odzwierciedla środowisko, pomijając drobne
szczegóły, które i tak nie wprowadzają znaczących trudności przy dalszych procesach
dopasowania.
7.4.2
Badania na obiekcie rzeczywistym
W kolejnym kroku wykonano serię skanów laserem SICK LMS200 na korytarzu budynku Wydziału Elektrycznego Politechniki Poznańskiej (rys. 7.6) odnotowując przybliżone przesunięcia i zmiany orientacji na podstawie pomiarów wykonanych taśmą mierniczą. Pomiary wykonywano na wysokości 93cm.
Rysunek 7.6: Zdjęcie korytarza budynku Wydziału Elektrycznego Politechniki
Poznańskiej.
42
Pierwszy pomiar, definiujący punkt początkowy mapy, wykonano w otoczeniu końca
korytarza ze skanerem skierowanym w stronę okien. Następnie wykonano w tym samym
miejscu trzy kolejne skany przy każdorazowym obrocie urządzenia o 90o . Dalsze pomiary wykonywano w podobny sposób (4 cykle wokół osi skanera) przesuwając urządzenie
wzdłuż korytarza o kolejno 4,5m, 1,9m, 4,3m, 1,75m i 3,3m. Co czwarty skan dokonywano
także rekonstrukcji mapy. Z 23 cykli pomiarowych uzyskano rezultat zaprezentowany na
rysunku 7.7.
Rysunek 7.7: Mapa otoczenia powstała ze złożenia skanów korytarza bez (po
lewej) i po usunięciu elementów o wartości nu =1.
Jak widać, mapie tej towarzyszy wiele „artefaktów”. Są one w głównej mierze rezultatem różnych właściwości optycznych skanowanych obiektów, a także niedokładności pomiarowych związanych z pozycją skanera. Mimo wszystko widać poprawny zarys środowiska.
Rozdział 8
Lokalizacja globalna przy pełnej
znajomości mapy otoczenia
W przypadku jawnej mapy otoczenia (pochodzącej, np. z wcześniejszego eksperymentu lub wiedzy a priori) robot może dokonać lokalizacji poprzez dopasowanie cech
otrzymanych z analizy skanu do prymitywów tworzących tę mapę. Wyróżnia się pojęcie
lokalizacji globalnej i lokalnej [XZZ03]. Pierwsza z nich polega na dopasowaniu cech ze
skanu do konkretnego miejsca na mapie oraz zgrubnej estymacji konfiguracji robota. Drugi
typ polega na śledzeniu poruszającego się w czasie robota i ciągłym wyznaczaniu pozycji
i orientacji na podstawie przemieszczających się landmarków. W tym rozdziale rozważa
się lokalizację globalną przy założeniu kompletnej znajomości mapy z jawnie określonym
układem współrzędnych. Zadanie to można określić mianem problemu korespondencji.
Ponieważ parametry uzyskanych w wyniku ekstrakcji odcinków (i/lub narożników)
odnoszą się do układu współrzędnych związanego z robotem, którego konfiguracji w przestrzeni
nie znamy, należy analizować relacje geometryczne pomiędzy nimi. Mapa globalna w
przeciwieństwie do lokalnej nie ma zwykle charakteru uporządkowanego, co niekorzystnie
wpływa na szybkość wykonywania algorytmu.
8.1
Odcinki w pełni widzialne
Z uzyskanego cyklu pomiarowego najpewniejszą informację niosą odcinki, które zostały
w pełni zeskanowane (CLS ang. Complete Line Segments), czyli te, które nie były w żaden
sposób zasłonięte innym obiektem i mieściły się w zasięgu pomiaru urządzenia. Zmierzony parametr długości takich odcinków jest bliski prawdziwemu, co stanowi pierwszy krok
przeszukiwania. Nie zawsze łatwo jest określić czy prymityw jest w pełni widoczny, a może
zdarzyć się, że skan nie zawiera ani jednej takiej cechy. Grupa punktów podzielona na
trzy lub więcej segmenów zawiera takie odcinki (niepewność dotyczy jedynie dwóch skrajnych). Odcinek na tle dwóch innych o podobnym parametrze ϕ także ku temu pretenduje.
43
44
8.1. Odcinki w pełni widzialne
W ogólności każdy prymityw, którego sąsiednie odcinki rozpoczynają się w
punkcie o większej lub równej odległości od początku układu współrzędnych
(O) niż jego własny punkt krańcowy należy traktować jako w pełni widzialne.
Przypadki te zobrazowano na rysunku 8.1.
Rysunek 8.1: Przykłady odcinków w pełni widzialnych.
Należy jednak uważać na przypadki, w których mierzona odległość wychodziła poza
zasięg skanera. Wówczas dwa sąsiadujące odcinki mogą rozdzielone być pustą przestrzenią
i, pomimo że nie zostały w całości zeskanowane, jeden z nich zostanie zakwalifikowany do
widzialnych. Przypadek ten można wyeliminować sprawdzając, czy punkty O, Pki oraz
Ppi+1 leżą w otoczeniu wspólnej prostej zgodnie z algorytmem nr 5.
Algorytm 5: TestWidoczności
1
f o r i = 2 : 1 : IloscPrymitywow −1
2
dPp1 = OdlegloscOdZera (Pp( i ) ) ;
3
dPk2 = OdlegloscOdZera ( Pk ( i − 1 ) ) ;
4
dPk1 = OdlegloscOdZera ( Pk ( i ) ) ;
5
dPp2 = OdlegloscOdZera (Pp( i + 1 ) ) ;
6
7
i f ( dPp1 <= dPk2 && dPk1 <= dPp2 )
8
[ A1 , B1 , C1 ] = P a r a m e t r y P r o s t e j ( Pk ( i − 1 ) ) ;
9
[ A2 , B2 , C2 ] = P a r a m e t r y P r o s t e j (Pp( i + 1 ) ) ;
10
11
d1 = O d l e g l o s c O d P r o s t e j (A1 , B1 , C1 , Pp( i ) ) ;
12
d2 = O d l e g l o s c O d P r o s t e j (A2 , B2 , C2 , Pk ( i ) ) ;
13
14
i f ( d1 < e p s i l o n && d2 < e p s i l o n )
15
16
17
Widoczny ( i ) = t r u e ;
end
else
8.2. Relacje geometryczne mapy lokalnej
18
45
Widoczny ( i ) = f a l s e ;
end
19
20 end
A, B, C to parametry prostej o równaniu postaci ogólnej, zaś epsilon jest pewną małą
wartością (w doświadczeniach przyjęto wartość 20mm), która określa przedział dozwolonych
odchyleń punktu środkowego od prostej. Zauważyć należy, że pętla for omija pierwszy
i ostatni odcinek. Dzieje się tak dlatego, że mają one tylko jednego sąsiada. Pierwszy i
ostatni prymityw mapy lokalnej mogą również być uznane za w pełni widzialne tylko w
przypadku, gdy pierwszy i ostatni pomiar ze skanu tworzą samodzielne grupy nie dające
odcinków. W celu uproszczenia algorytmu krok ten został jednak pominięty.
Zatem już na poziomie ekstrakcji należy określić, czy uzyskany odcinek jest lub też nie
jest w pełni widzialny. W tym celu strukturę prymitywów należy rozszerzyć o dodatkową
tablicę typu bool, w której przechowywana będzie ta informacja.
8.2
Relacje geometryczne mapy lokalnej
Informacja o pojedynczych odcinkach jest niedostateczna by jednoznacznie ocenić konfigurację robota na mapie globalnej. Z tego względu należy rozszerzyć strukturę mapy
lokalnej o relacje pomiędzy tworzącymi ją figurami. W tym celu wybiera się dowolny z
pośród odcinków w pełni widzialnych (np. nadłuższy) i oznacza go jako figurę odniesienia
(reprezentanta). Dla każdego kolejnego segmentu typu CLS obliczamy:
• ∆Pc - odległość między jego środkiem, a środkiem odcinka odniesienia,
• ∆ϕ - różnicę kąta pomiędzy prostymi wspierającymi obu odcinków,
• ∆L - różnicę długości odcinków,
• α - kierunek wektora Pc0 − Pci .
Warunkiem koniecznym jest występowanie więcej niż jednego segmentu w pełni widocznego.
W wyniku tej operacji powstaje tablica o wymiarze 4×(n-1), gdzie n oznacza liczbę odcinków CLS.
W następnym kroku na mapie globalnej odszukane zostają odcinki korespondujące z
obranym przez nas reprezentantem. Operacji tej dokonuje się w oparciu o informację o
długości figury. W rezultacie może okazać się, że znalezionych odpowiedników jest więcej
niż jeden. Wówczas dla każdego z nich należy z osobna odszukiwać odcinków mapy globalnej, spełniających względem reprezentanta te same relacje porównawcze.
46
Rysunek 8.2: Schematyczne przedstawienie relacji między odcinkami.
Należy zwrócić uwagę na problem obiektów symetrycznych. Może bowiem okazać się,
że zarówno różnice długości, odległości między środkami oraz różnice kąta mają niemalże
równe wartości dla dwóch różnych odcinków. Przykład taki zobrazowano na rysunku 8.3.
Rysunek 8.3: Problem symetrii w procesie dopasowania cech.
Przy założeniu, że P0 wybrany został jako reprezentant, P1 i P2 będą miały względem
niego takie same wartości ∆Pc , ∆ϕ oraz ∆L. Są to zatem warunki niedostateczne aby w
dalszych krokach jednoznacznie sklasyfikować odcinek mapy lokalnej jako adekwatny korespondent mapy lokalnej. Problem ten rozwiązać można w oparciu o wartości α. Oprócz
różnicy między orientacją prostych wspierających oba odcinki, również różnica między
kątem α dla danego odcinka typu CLS i kątem ϕ prostej wspierającej reprezentanta musi
być zachowana. Należy zwrócić uwagę, że wartość bezwzględna tej różnicy musi znajdować się w przedziale h0o ,90o ).
Te cztery warunki jednoznacznie opisują relację między dwoma odcinkami i dopuszczają
do klasyfikacji jedynie słuszne prymitywy. Algorytm 6 przedstawia sposób odnajdywania
korespondujących odcinków CLS wśród obiektów mapy globalnej.
Algorytm 6: AnalizaOdcinkówKorespondujących
1
f o r i = 1 : 1 : NumberOfCorrespondingLines
2
47
f o r j = 1 : 1 : NumberOfRelatedCLS
f o r k = 1 : 1 : NumberOfMapPrimitives
3
4
W1 = f a l s e ; %dPc c o n d i t i o n
5
W2 = f a l s e ; %dPhi c o n d i t i o n
6
W3 = f a l s e ; %dL c o n d t i t i o n
7
W4 = f a l s e ; %dPhiAlpha c o n d i t i o n
8
9
MapDPc = DistanceBetween (MapPc( i ) , MapPc( k ) ) ;
10
MapDPhi = MapLinePhi ( i ) − MapLinePhi ( k ) ;
11
MapDL = MapLineL ( i ) − MapLineL ( k ) ;
12
MapDPhiAlpha = MapLinePhi ( i ) − MapLineAlpha ( k ) ;
13
i f ( abs ( LocDPc ( j ) − MapDPc) < DPcmin )
14
15
W1 = t r u e ;
16
end
17
i f ( abs ( LocDPhi ( j ) − MapDPhi ) < DPhimin ) )
18
W2 = t r u e ;
19
end
20
i f ( abs ( LocDL ( j ) − MapDL) < DLmin)
21
W3 = t r u e ;
22
end
23
i f ( abs ( LocDPhiAlpha ( j ) − MapDPhiAlpha ) < DLmin)
24
W4 = t r u e ;
end
25
26
i f (W1 && W2 && W3 && W4)
27
28
MatchingObject ( j , i ) = k ;
29
MatchingFound ( i ) = MatchingFound ( i ) + 1 ;
end
30
end
31
32
end
33 end
W powyższym algorytmie NumberOfCorrespondingLines oznacza liczbę odnalezionych
odcinków korespondujących do wybranego reprezentanta, NumberOfRelatedCLS to liczba
pozostałych odcinków mapy lokalnej w pełni widzianych, wszystkie zmienne o nazwach
zaczynających się od Map dotyczą odcinków mapy globalnej, natomiast zaczynające się od
Loc, mapy lokalnej.
8.3. Estymacja konfiguracji skanera laserowego
48
W tabeli MatchingObject znajdować się będą numery segmentów, które spełniły
warunki dopasowania. Wektor MatchingFound jest w początkowym kroku wyzerowany.
Za każdym razem, gdy dla analizowanego odcinka korespondującego z figurą odniesienia
znaleziona zostanie relacja pomiędzy innym odcinkiem mapy globalnej, która zbliżona
jest do odpowiadającej relacji na mapie lokalnej, wartość w zadanym polu podlega inkrementacji. W ten sposób na końcu przeszukiwania można określić, który z odnalezionych
odcinków korespondujących ma najwięcej wspólnych relacji z mapą lokalną.
Rysunek 8.4: Mapa globalna i skan z odnalezionym dopasowaniem.
Należy mieć także na uwadze, że warunek W2 został tu opisany w sposób uproszczony i nie jest wystarczający ze względu na występowanie okresowości jednostki kątowej.
Dla zobrazowania tego problemu rozważyć można następujący przypadek. Niech odcinek
odniesienia ma parametr ϕ0 = 0o , natomiast dwa inne segmenty kolejno ϕ1 = 359o i
ϕ2 = 1o . Geometrycznie rzecz biorąc, oba są do siebie bardzo zbliżone, lecz z ściśle
matematycznego punktu widzenia obliczone różnice będzie dzieliła „przepaść”. Co więcej
różnica ta zależna jest od punktu odniesienia (zarówno mapy jak i skanera). Wprowadza
to pewnego rodzaju symetrię i przykładowo wynikający kąt między odcinkami według
mapy globalnej będzie wynosił 90o natomiast według mapy lokalnej 270o (patrz rys. 8.5).
Należy uwzględnić wszystkie te możliwości przy projektowaniu warunku zgodności fazy.
8.3
Estymacja konfiguracji skanera laserowego
Po określeniu najbardziej trafnego korespondenta odcinka odniesienia na mapie globalnej należy wyznaczyć orientację robota względem układu z nią związanego. W tym
celu wykorzystuje się obliczone parametry α pomiędzy odcinkami CLS mapy globalnej i
lokalnej. Różnica tych dwóch kątów daje w wyniku orientację robota. Poniżej przedstaw-
8.4. Zawężenie zakresu pomiaru na podstawie wiedzy a priori
Rysunek 8.5:
odniesienia.
49
Różnice parametru ϕ w zależności od umiescowienia punktu
iono wzór uśredniający wyniki ze wszystkich zebranych relacji.
n
θr =
1X
αgi − αli ,
n
(8.1)
i=0
gdzie
α h0o ,360o ).
Przy implementacji pamiętać należy, że kąt 0o i 360o przedstawiają tę
samą orientację, lecz ich średnia to 180o . Rozwiązanie tego problemu opisano w dodatku
A. Pozycja robota (Xr ,Yr ) jest wówczas wynikiem prostych przekształceń geometrycznych:
Xr = Xcg − (Xcl cos θr + Ycl sin θr ),
(8.2)
Yr = Ycg − (Ycl cos θr − Xcl sin θr ),
gdzie Xcl ,Ycl to współrzędne środka odcinka CLS na mapie lokalnej, zaś Xcg ,Ycg współrzędne
środka odpowiadającego odcinka na mapie globalnej.
8.4
Zawężenie zakresu pomiaru na podstawie wiedzy a
priori
Dla niektórych, szczególnych przypadków mapa globalna może przyjmować charakter powtarzających się struktur. Z punktu widzenia lokalizacji globalnej jest to cecha
wyjątkowo niekorzystna. Prowadzona lokalizacja może bowiem dawać w efekcie grupę konfiguracji spełniających warunki poprawnej klasyfikacji. Jedynym rozwiązaniem tego problemu jest informacja o zgrubnej pozycji robota, która pozwala zawężyć pole przeszukiwań
(ROI, ang. Region Of Interest). Może być to informacja o pomieszczeniu, w którym robot
aktualnie się znajduje. Wówczas podczas dopasowywania cech między mapami: lokalną
i globalną należy odrzucić wszelkie odcinki, które wychodzą poza ramy tego pomieszczenia.
Wiedzę tę wykorzystać należy już przy odnajdywaniu odcinka korespondującego z
reprezentantem mapy lokalnej. Każdy dodatkowy odcinek na tym poziomie przekłada się
na znaczącą eskalację ilości obliczeń. Co więcej, warunek zawierania się wewnątrz ROI
50
warto sprawdzać przy okazji przeszukiwania mapy globalnej w celu odnalezienia odcinków
spełniających relacje porównawcze. W tym celu sprawdzamy czy punkt centralny analizowanego odcinka znajduje się w danym przedziale. Trzecim zastosowaniem jest estymacja
pozycji samego robota, która również nie może wybiegać poza wskazane ramy.
Gdy mamy do czynienia z konkretną pozycją, znaną, np. z zewnętrznego systemu
pomiarowego, która charakteryzuje się dużą niedokładnością i której rozkład statystyczny jest znany, wówczas ROI możemy wyznaczyć jako obszar w promieniu 8000mm
(zasięg skanera SICK LMS200) natomiast estymowana pozycja powinna znajdować się
w otoczeniu 3σ punktu wskazanego przez zewnętrzny system pomiarowy (rys.
8.6).
Sprawdzenie warunku przynależności wykorzystuje w tym celu twierdzenie Pitagorasa,
Rysunek 8.6:
odniesienia.
Różnice parametru ϕ w zależności od umiejscowienia punktu
przy czym współrzędne środka odcinka, czy uzyskanej pozycji robota określają długości
przyprostokątnych, natomiast promień ROI, czy odpowiednio 3σ określa długość przeciwprostokątnej.
8.5
Wyniki eksperymentalne
Poniżej przedstawiono zestawienie części przeprowadzonych eksperymentów zarówno
symulacyjnych jak i rzeczywistych. Wyniki przedstawiają uśrednioną wartość estymat
konfiguracji robota, na którą składają się pozycje oszacowane względem różnych odcinków
w pełni widzialnych. Średni czas przeszukiwania mapy w celu znalezienia dopasowania
wahał się między 0,15s a 0,20s.
8.5.1
Badania symulacyjne
Do początkowego przetestowania algorytmu lokalizacji wykorzystano uzyskaną wcześniej
mapę globalną (7.5) i wykonano serię pomiarów w różnych punktach wirtualnego środowiska.
51
Punkty te dobierane były tak, aby w każdym przypadku odnajdywane były co najmniej
dwa odcinki w pełni widzialne. Poniższe wyniki przedstawiane są jako wykonany pomiar
(po lewej) oraz wynik lokalizacji (po prawej). W opisach rysunków znajdują się informacje
o konkretnych wartościach rzeczywistych i oszacowanych wynikach.
Rysunek 8.7: Wyniki:
X,Y,θ rzeczywiste: 12000mm, -4500mm, 225o
X,Y,θ oszacowane: 11998,3mm, -4504,3mm, 225,19o
X,Y,θ rzeczywiste: 4200mm, -5500mm, 180o
X,Y,θ oszacowane: 4200,9mm, -5501,4mm, 180,21o
8.5.2
Badania na obiekcie rzeczywistym
W następnym kroku wykorzystano mapę zbudowaną z rzeczywistych pomiarów wykorzystanych na korytarzu (rys. 7.7) i podjęto próbę dopasowania cech z pojedynczego skanu
uzyskanego w wyniku pomiaru w tym samym otoczeniu. Znacznie częściej niż w przypadku symulacyjnym algorytm nie radził sobie z poprawnym dopasowaniem aczkolwiek
również pojawiały się pozytywne wyniki. Na obrazach 8.11, 8.12 przedstawiono mapę
lokalną (po lewej) oraz mapę globalną ze wskazaną estymowaną konfiguracją robota (po
prawej).
8.6. Problemy związane z lokalizacją globalną
52
X,Y,θ rzeczywiste: -1000mm, -1650mm, 0o
X,Y,θ oszacowane: -1000,2mm, -1641,8mm, 0,46o
X,Y,θ rzeczywiste: 4300mm, -4200mm, 22,5o
X,Y,θ oszacowane: 4300,1mm, -4199mm, 22,41o
8.6
Problemy związane z lokalizacją globalną
Dużym problemem okazują się środowiska silnie ustrukturyzowane w sposób symetryczny.
Wykonanemu pomiarowi może wówczas odpowiadać więcej niż jedna konfiguracja skanera. W takim przypadku jedynym rozwiązaniem jest posiadanie wiedzy a priori o zgrubnej
pozycji lub orientacji robota.
Sam pomiar może okazać się zbyt ubogi w informacje potrzebne do estymacji pozycji robota. Jeżeli robot ma rozpocząć samodzielną pracę i w początkowej fazie nie może
się zlokalizować, należy wykonać pełen obrót wokół własnej osi i w ten sposób próbować
wykonać skan dający rzetelne wyniki. Warto rozważyć fakt, że wyjście pomiaru poza zasięg
także niesie pewną informację, którą można wykorzystać w celach lokalizacyjnych. Odcinki
nie widzialne w pełni nadal niosą informację o różnicy kątów pomiędzy nimi, a parametr
długości, mimo iż niepełny, może znacząco zawężyć zakres przeszukiwania odpowiadających prymitywów, poprzez eliminację tych, o mniejszej wartości tego parametru.
X,Y,θ oszacowane: -319,36mm, -7776,2mm, 319.6o
X,Y,θ oszacowane: -803,17mm, -4307,3mm, 11,8o
53
54
Błędne oszacowanie lokalizacji może doprowadzić do „eksplozji” danych uzyskiwanych
w procesie fuzji z różnych systemów pomiarowych. Z tego względu należy szczególny nacisk
nałożyć na zabezpieczenie pewności otrzymanego wyniku. W rozważanym przypadku
należy kierować się zasadą, że wynikowy brak danych jest bardziej korzystny niż dane
niepoprawne.
Rozdział 9
Jednoczesna lokalizacja i
mapowanie
W poprzednich rozdziałach zagadnienia lokalizacji oraz mapowania omówione zostały
w sposób niezależny, przy założeniu znajomości odpowiednich informacji, wymaganych
do realizacji zadania. Wymogi te istotnie redukują autonomię robota mobilnego gdyż on
sam, bez odpowiednich narzędzi zwykle nie jest w stanie ich spełnić. Maszyna znajdująca się w nieznanym środowisku, posiadająca narzędzie mapujące teren oraz co najwyżej
zestaw odometryczny, jest w stanie w sposób precyzyjny i nie rozbiegający się w czasie
dokonywać również samolokalizacji.
Zagadnienie SLAM jest pojęciem ogólnym, które może być realizowane na wieloraki
sposób. Projekty różnią się ze względu na wykorzystane systemy sensoryczne, sposób
przetwarzania sygnałów pomiarowych, późniejszą fuzję danych oraz metodę reprezentacji
mapy otoczenia. Na łamach tej pracy rozważania na temat SLAM dotyczyć będą dwóch
przypadków: robota wyposażonego w skaner laserowy i odometrię inkrementalną oraz
robota wyposażonego jedynie w skaner laserowy.
Niniejszy rozdział nie stanowi ostatecznego rozwiązania problemu jednoczesnego mapowania i lokalizacji, lecz jest jedynie wstępem do tego zagadnienia, który ma wskazać potencjalny kierunek jaki można obrać w celu rozwinięcia projektu.
9.1
Robot wyposażony w skaner laserowy i odometrię
Budowanie mapy globalnej opisane w rozdziale 7 zakładało pełną znajomość stanu
robota w momencie wykonywania cyklów pomiarowych skanerem. System odometryczny
dostarcza tej informacji ale ponieważ błąd jakim jest obarczona narasta w czasie, nie
można uwzględniać jej jako pewnej. Należy zatem każdorazowo przeprowadzać korekcję
owej estymaty w oparciu o dane pochodzące ze skanera laserowego oraz aktualnego stanu
55
9.2. Robot wyposażony jedynie w skaner laserowy
56
mapy. Z jednej strony błędy mapy wyraźnie skorelowane są z błędem estymowanego stanu
robota, a z drugiej strony ten ostatni zależny jest od poprawności stanu mapy (ang. Egg
and Chicken Problem).
Przy częstości skanowania dużej w stosunku do prędkości ruchu robota przeprowadzenie lokalizacji na podstawie skanu zwykle nie stanowi większego problemu. Uzasadnione
jest to tym bardziej, że zakres przeszukiwania ograniczony jest otoczeniem lokacji robota wskazywanej przez odometrię. Dysponując dwoma wektorami konfiguracji robota:
oszacowanym na podstawie odometrii i w procesie analizy skanu, należy je odpowiednio
uśrednić i oszacować względny błąd, który posłuży następnie do korekcji estymaty odometrycznej. Posiadając w taki sposób uśredniony stan robota dokonuje się procesu aktualizacji mapy, której warunki dopasowania cech powinny odfiltrować pozostający błąd.
Możemy proces ten zapisać jako następująca sekwencja:
1. Predykcja stanu robota w chwili (i) na podstawie odometrii.
2. Wykonanie skanu i lokalizacja względem mapy globalnej z chwili (i-1).
3. Oszacowanie wag niepewności dla wyniku odometrii i lokalizacji ze skanu.
4. Uśrednienie obu wyników z uwzględnieniem obliczonych wag.
5. Aktualizacja stanu mapy na podstawie skanu i estymowanego stanu robota.
Podczas implementacji należy zapewnić odpowiednią synchronizację wykonywania pomiarów. Waga wyniku odometrii powinna maleć w sposób odwrotnie proporcjonalny do
przebytej drogi między kolejnymi iteracjami algorytmu natomiast waga estymaty lokacji
uzyskanej ze skanu powinna zależna być od ilości prymitywów wykorzystanych w procesie
dopasowania cech map lokalnej i globalnej.
Do tego typu fuzji danych wykorzystywane są powszechnie znane i stosowane metody
takie jak m.in. rozszerzony filtr Kalmana (EKF, ang. Extended Kalman Filter ), filtracja
cząsteczkowa (ang. Particle Filter ) czy filtr Bayesa. Są one skuteczne ze względu na
uwzględnianie modeli statystycznych wykorzystanych systemów pomiarowych w procedurach uśredniania. EKF ogranicza możliwość stosowania modelu błędu jedynie o rozkładzie
normalnym (co w rozważanym przypadku zwykle wystarcza), filtracja cząsteczkowa omija
ten problem choć wymaga większych nakładów obliczeniowych.
9.2
Robot wyposażony jedynie w skaner laserowy
W przypadku gdy robot nie jest wyposażony w system odometryczny, a znamy jego
przybliżony model dynamiki, wektor stanu estymować możemy poprzez odpowiednie całkowanie
sygnałów sterujących i postępować tak, jak w przypadku punktu 9.1. Jeżeli jednak nie
9.3. Optymalizacja procesu SLAM
57
mamy tej informacji, polegać możemy jedynie na sygnale ze skanera laserowego.
W momencie rozpoczęcia działania algorytmu wykonywany jest skan, który po analizie
jednocześnie przedstawia mapę lokalną i globalną dla pierwszej iteracji oraz qr1 = [0 0 0].
Druga iteracja niesie kolejny cykl pomiarowy, który podlega ekstrakcji cech. Te podlegają
procesowi dopasowania ze stanem mapy globalnej z poprzedniego kroku i na podstawie
uzyskanych korespondencji określane zostaje qr2 = [θr2 Xr2 Yr2 ]. W oparciu o te dane zaktualizowana zostaje mapa globalna.
Należy mieć na uwadze, że zbyt duża prędkość ruchu robota (zwłaszcza obrotowa)
może doprowadzić do nierozpoznania cech i braku informacji o jego lokacji. Nie oznacza
to konieczności zatrzymania robota, ale nie jest on w stanie dalej aktualizować początkowej
mapy globalnej. Wówczas rozwiązaniem jest rozpoczęcie budowy drugiej mapy, przy jednoczesnym szukaniu dopasowania cech ze skanu na obu z nich. Jest to znaczące obciążenie
obliczeniowe, lecz może w rezultacie doprowadzić do scalenia obu map i dalszej nieprzerwanej pracy.
9.3
Optymalizacja procesu SLAM
Głównym obciążeniem algorytmu SLAM jest dopasowanie cech do mapy globalnej i
rośnie ono wraz z jej rozwojem. Procedury tej wymaga zarówno lokalizacja robota jak
i aktualizacja mapy. Stan robota estymowany jest na podstawie przyrównania orientacji i środków geometrycznych korespondujących prymitywów i uśrednienia otrzymanych
wartości. W poprzednich punktach algorytm zakładał wykonanie kompletnej lokalizacji
i ewentualne uśrednienie wyniku z wartością predykowaną, a dopiero następnie zaktualizowanie mapy. Aby nie dokonywać ponownie procesu dopasowania, należy strukturę cech
mapy lokalnej rozszerzyć o wskaźnik na obiekt mapy globalnej, z którym dana cecha korespondowała w procesie lokalizacji.
Ponieważ ruch robota jest przebiegiem ciągłym i zakłada się nieprzerwaną pracę algorytmu, dopasowanie cech w celu lokalizacji robota można uprościć zawężając przedział
przeszukiwanych prymitywów mapy globalnej. Jeżeli do czynienia mamy z mapą uporządkowaną, zakres ograniczamy do odcinków (ewentualnie także wierzchołków), które brały
udział w poprzedniej iteracji oraz, np. dwóch kolejnych sąsiadów z obu stron przedziału.
Należy jednak uwzględnić możliwość wejścia pomiaru skanera w zasięg innego pomieszczenia,
w przypadku którego powyższe ograniczenie nie będzie słuszne. Jeżeli dana jest nam informacja o predykowanym stanie robota na chwilę wykonania pomiaru, przedział poszukiwań
ograniczamy w sposób geometryczny jako półkole o promieniu równym zasięgu skanera
oraz zwrocie zgodnym z szacowaną orientacją (rys. 9.1). W przypadku braku tej informacji posłużyć się można wiedzą o maksymalnej lub ostatnio zmierzonej prędkości robota i
9.3. Optymalizacja procesu SLAM
58
w ten sposób określić możliwe ograniczenia przeszukiwania. Należy rozważnie podchodzić
Rysunek 9.1: Zakres przeszukiwań dla znanej predykcji stanu robota.
do tego typu usprawnień i pamiętać, że w początkowych fazach działania algorytmu mapa
charakteryzuje się małą złożonością, co nie wymaga podejmowania radykalnych kroków
optymalizacyjnych.
Rozdział 10
Podsumowanie
10.1
Wnioski
W pracy zaprezentowano i rozwiązano problem mapowania terenu oraz lokalizacji
robota przy wykorzystaniu skanera laserowego. Skuteczność tych rozwiązań potwierdzono wynikami eksperymentalnymi przeprowadzonymi zarówno w świecie wirtualnym jak
i rzeczywistym.
Mapa wektorowa, którą zdecydowano się wykorzystać sprawdza się w omawianych
celach zarówno pod względem precyzji opisu jak i zapotrzebowania na moc obliczeniową
i pamięć. W opisanym rozwiązaniu opiera się ona jedynie na prymitywach o postaci odcinków. Według kryterium dokładności nie jest to postać optymalna, lecz dla sporego
podzbioru zadań wystarczająca. Jak wspomniano w początkowych rozważaniach, trudnością map wektorowych jest rejestrowanie obiektów dynamicznych.
Proces budowy mapy globalnej przy znanych i pewnych wartościach wektora konfiguracji robota jest zadaniem stosunkowo prostym, a jedyną trudność stanowi dobranie
poprawnych przedziałów nieufności parametrów prymitywów tak, aby odfiltrowane zostały
wszelkie błędy powstałe wskutek niedokładności pomiarowych skanera. W pracy przedstawiono także różne techniki korekcji niektórych błędów, które ujednolicają i poprawiają
stan mapy.
Jednoznaczna lokalizacja globalna robota na mapie bez zgrubnej wiedzy o pozycji jest
możliwa jedynie dla środowisk niesymetrycznych. W zaproponowanej metodzie zakłada
się istnienie odcinków w pełni widzialnych dostępnych w wyniku ekstrakcji cech ze skanu,
co stanowi spore ograniczenie jej stosowalności. Istnieją jednak możliwości usprawnienia
tego procesu, które rozszerzą zbiór dopuszczalnych konfiguracji robota dających się jednoznacznie zlokalizować.
59
10.2. Dalszy rozwój projektu
60
Temat jednoczesnej lokalizacji i mapowania został podjęty, aby wskazać na możliwe
podejście do rozwiązania tego problemu. Splecienie obu zagadnień jest jednym z kluczowych aspektów stojących na drodze do usamodzielnienia pojazdu.
W trakcie implementacji pojawiło się wiele problemów natury inżynierskiej, które
pokazują stopień złożoności rozważanego tematu. Między innymi wiele trudności sprawiły zadania programowe związane z wartościami kątowymi (patrz Dodatek A). Zarówno
budowa mapy jak i proces lokalizacji odnoszą się do wielu składników losowych, gdyż sam
pomiar jest procesem stochastycznym. Z tego względu sprawdzane warunki wymagają w
większości określenia dopuszczalnych odchyleń, które zostały wyznaczone w sposób empiryczny. Przedziały te wspierają proces budowy mapy w przypadku konfiguracji robota
obarczonej niewielkim błędem, gdyż cechy uzyskane w procesie ekstrakcji są w pewnym
stopniu „przyciągane” przez prymitywy znajdujące się już na mapie.
10.2
Dalszy rozwój projektu
Kontynuacja i rozwój projektu wymaga usprawnienia niektórych z rozważanych na łamach tej pracy zagadnień oraz generuje nowe problemy, które należy rozwiązać. Poprawne
działanie algorytmu zależne jest w dużej mierze od zabezpieczeń, nałożonych na estymatory zarówno konfiguracji robota, jak i stanu mapy. Z drugiej strony efektywne wykorzystanie go w czasie rzeczywistym ogranicza liczbę wykonywanych obliczeń.
Problemami związanymi z budową mapy, jakimi należy zająć się w następnym kroku
jest jednoznaczne określenie przestrzeni roboczej i przestrzeni niejawnej.
Pozwoli to
robotowi podejmować decyzje o tym, w którym kierunku ma się poruszać tak, aby mapa
była kompletna (brak przestrzeni niejawnych nie odgrodzonych przeszkodą). Krok ten
miałby także praktyczne zastosowanie wizualizacyjne.
Innym ważnym aspektem jest rozszerzenie modelu mapy o informację o narożnikach
wspomnianych w sekcji 6.4.2. Mając informację o przestrzeni roboczej pośród mapy łatwo
jest zidentyfikować faktyczny kąt rozwarcia narożnika oraz jego zorientowanie. Taka struktura mapy (zarówno globalnej jak i lokalnej) daje większe możliwości dopasowania cech
gdyż operujemy wówczas na większej ilości danych. Przykładowo na mapie lokalnej może
znajdować się tylko jeden odcinek typu CLS lecz kilka narożników. Estymacja pozycji na
podstawie relacji między wszystkimi tymi prymitywami jest znacznie bardziej wiarygodna.
W celu przyspieszenia działania algorytmu dopasowania mapy lokalnej do globalnej
warto tą ostatnią budować w sposób uporządkowany, tzn. w taki, że sąsiednie odcinki
mają przyporządkowane kolejne liczby naturalne. Wówczas dla znalezionych odcinków ko-
10.2. Dalszy rozwój projektu
61
respondujących z wybranym reprezentantem mapy lokalnej nie trzeba przeszukiwać całej
mapy globalnej lecz jedynie pewne ich sąsiedztwo.
W końcu, należy połączyć zagadnienie mapowania i lokalizacji aby działały w sposób
komplementarny, realizując ostatecznie algorytm SLAM.
Dodatek A
Problemy związane z okresowością
kąta
W pracy wielokrotnie posługiwano się zmiennymi o wymiarze kątowym, choćby do
opisu orientacji robota, prostych wspierających odcinki czy kierunku danego wektora. Założono, że miara kąta mieści się w zakresie α ∈ h0o ,360o ), a wszelkie od tego odstępstwa
wynikające z obliczeń, należy natychmiast korygować.
Przykładowo, wewnętrzna funkcja Matlaba atan2() daje w wyniku kąty z zakresu
h−180o ,180o ) (oznaczenie 180o stosowane będzie w algorytmach jako π). Często obliczane
sumy i różnice również kątów mogą sięgać poza ten zakres. Z tego względu należy w
odpowiednich miejscach kodu sprawdzać poprawność tych wartości zgodnie z następującym warunkiem:
i f ( p h i < 0 ) p h i = p h i + 2∗ pi ;
e l s e i f ( p h i >= 2∗ pi ) p h i = p h i − 2∗ pi ;
Inny problem stanowi porównywanie dwóch zmiennych o wymiarze kątowym. Warunek
różnicy w sensie wartości bezwzględnej jest w tym przypadku niewystarczający. Dwa kąty
bliskie granicom przedziału są bowiem sobie bardzo bliskie, lecz w sensie algebry liczb
rzeczywistych dzieli je prawie 360o . Zatem oprócz sprawdzenia różnicy obu kątów, należy
także sprawdzić tę samą różnicę powiększoną i pomnieszoną o wartość 360o .
i f ( abs ( p h i 1 − p h i 2 ) < e p s i l o n ) W1 = t r u e ; end
i f ( abs ( p h i 1 − p h i 2 + 2∗ pi ) < e p s i l o n ) W2 = t r u e ; end
i f ( abs ( p h i 1 − p h i 2 − 2∗ pi ) < e p s i l o n ) W3 = t r u e ; end
i f (W1 | | W2 | | W3) W = t r u e ; end
Problem okresowości daje się również we znaki w przypadku uśredniania kilku wartości
kątowych. Wartość średnia trzech kierunków: 0o , 0o i 360o daje w wyniku 120o , a zatem
62
Problemy związane z okresowością kąta
63
orientację kompletnie odmienną od poprawnej wartości uśrednionej. Poprawny algorytm
uśredniania jest nieco bardziej rozwinięty niż jedynie suma podzielona przez ilość.
PHIsr = p h i ( 1 ) ;
i f ( I l o s c > 1)
f o r ( i = 1 : 1 : I l o s c −1)
i f ( abs ( p h i ( 1 ) − p h i ( i +1)) > pi && p h i ( 1 ) > pi )
PHIsr = PHIsr + p h i ( i +1) + 2∗ pi ;
e l s e i f ( abs ( p h i ( 1 ) − p h i ( i +1)) > pi && p h i ( 1 ) < pi )
PHIsr = PHIsr + p h i ( i +1) − 2∗ pi ;
else
PHIsr = PHIsr + p h i ( i +1);
end
end
end
PHIsr = PHIsr / I l o s c ;
Dodatek B
Obsługa środowiska symulacyjnego
Zaprojektowane na potrzeby badawcze środowisko symulacyjne składa się z kilku
plików, których nazwy jednoznacznie wskazują przydzielone im zadania. W zależności
od zadania, które chcemy wykonać, należy wyegzekwować jeden spośród dwóch plików.
Dla zadania budowy mapy globalnej jest to plik goMapping.m, natomiast dla zadania
lokalizacji globalnej - goLocalisation.m.
W obu przypadkach w pierwszej kolejności wykonywana jest inicjalizacja, w której
ustawiane są wartości dotyczące parametrów skanera i błędów pomiarowych oraz zakresy dopuszczalnych odchyleń dla warunków porównawczych. W wyniku budowy mapy
wyświetlane zostają: mapa rzeczywista oraz ta, otrzymana po złożeniu przeprowadzonych
skanów. Proces lokalizacji globalnej wyświetla mapę globalną z zaznaczoną rzeczywistą i
oszacowaną pozycją i orientacją skanera w środowisku. Dodawanie nowych przeszkód do
sztucznego środowiska zostało omówione w podrozdziale 5.2.
64
Dodatek C
Praca ze skanerem laserowym
SICK LMS200
Poniżej przedstawiono kluczowe informacje dotyczące pracy ze skanerem SICK LMS200
w sposób niestandardowy (bez firmowego oprogramowania).
Komunikacja z urządzeniem odbywa się poprzez łącze szeregowe z dostępnymi prędkościami transmisji danych 9600baud, 19200baud, 38400baud oraz 500kbaud. Po włączeniu,
standardowo ustawiona jest prędkość najniższa. Pierwsze trzy prędkości wymagają interfejsu RS232 lub nowszego i są zdefiniowane jako standardowe w systemie Windows.
Prędkość 500kbaud wymaga użycia interfejsu RS422 i odpowiedniego skonfigurowania
systemowego. W tym celu należy odnaleźć w rejestrze wpis odpowiadający za prędkości
przesyłu danych portem szeregowym i zamienić jedną z prędkości standardowych na 500
000. Wykonuje się to poprzez podanie dzielnika częstości wzorcowej (3MHz), czyli w
tym przypadku liczby 6. Podczas konfiguracji portu podajemy jednak wartość pierwotnej
prędkości, którą zamieniliśmy.
Zmianę prędkości pracy urządzenia w sposób programowy dokonujemy poprzez wysłanie
odpowiedniego rozkazu. W tabeli C.1 zestawiono ramki odpowiadające danym prędkościom oraz telegramy, które zostają przesłane do komputera po zrealizowaniu zmiany prędkości. W celu uwieńczenia tego procesu wymagane jest odebranie tych telegramów.
Tablica C.1: Wybór szybkości transmisji danych.
Szybkość transmisji danych
9600
19200
38400
500000
Zawartość ramki (heksadecymalnie)
02 00 02 00 20 42 52 08
02 00 02 00 20 41 51 08
02 00 02 00 20 40 50 08
02 00 02 00 20 48 58 08
65
Telegram zwrotny (heksadecymalnie)
06 02 81 03 00 A0 00 10 36 1A
06 02 81 03 00 A0 00 10 36 1A
06 02 81 03 00 A0 00 10 36 1A
06 02 81 03 00 A0 00 10 36 1A
66
Praca ze skanerem laserowym SICK LMS200
Urządzenie działa w pięciu różnych trybach pomiarowych, przedstawionych w tabeli
C.2. Po włączeniu, jako standardowy ustawiony jest tryb T5.
Tablica C.2: Wykaz trybów pracy urządzenia.
Tryb
T1
T2
T3
T4
T5
Zasięg kątowy (o )
0-100
0-100
0-100
0-180
0-180
Rozdzielczość kątowa (o )
1
0,5
0,25
1
0,5
Liczba próbek
101
201
401
181
361
Zmianę trybu również dokonuje się poprzez wysłanie odpowiedniego rozkazu do skanera. Ich wykaz przedstawiono w tabeli C.3. Podobnie jak przy zmianie prędkości, również
po zmianie trybu należy odebrać telegramy zwrotne.
Tablica C.3: Zmiana trybu pracy urządzenia.
Tryb
T1
T2
T3
T4
T5
Zawartość ramki (heksadecymalnie)
02 00 05 00 3B 64 00 64 00 1D 0F
02 00 05 00 3B 64 00 32 00 B1 59
02 00 05 00 3B 64 00 19 00 E7 72
02 00 05 00 3B B4 00 64 00 97 49
02 00 05 00 3B B4 00 32 00 3B 1F
Telegram zwrotny (heksadecymalnie)
06 02 81 07 00 BB 01 64 00 64 00 10 4A 3F
06 02 81 07 00 BB 01 64 00 32 00 10 12 92
06 02 81 07 00 BB 01 64 00 19 00 10 BE C4
06 02 81 07 00 BB 01 B4 00 64 00 10 5E B2
06 02 81 07 00 BB 01 B4 00 32 00 10 06 1F
Rozpoczęcie pracy inicjowane jest wysłaniem rozkazu START (02 00 02 00 20 24 34
08). Cykle pomiarowe są wykonywane w pętli do momentu wysłania ramki STOP (02
00 02 00 20 25 35 08) lub odłączenia zasilania. W zależności od trybu, w którym skaner
aktualnie pracuje, urządzenie przesyła pomiar nadając mu inny nagłówek. W tabeli C.4
zestawiono nagłówki pomiarów odpowiadające danym trybom pracy.
Tablica C.4: Wykaz nagłówków dla poszczególnych trybów pracy.
Tryb
T1
T2
T3
T4
T5
Zawartość nagłówka (heksadecymalnie)
02 80 CE 00 B0 65 40
02 80 96 01 B0 C9 40
02 80 26 03 B0 91 41
02 80 6E 01 B0 B5 40
02 80 D6 02 B0 69 41
Pojedyncza paczka danych składa się z 7 bajtów nagłówkowych, n bajtów danych pomiarowych oraz 3 bajtów statusu końcowego i sumy kontrolnej. Liczba n zależna jest od
wybranego trybu, a dokładniej od ilości zmierzonych odległości przypadających na skan.
Każda zmierzona odległość zapisywana jest w formie 2 bajtów. Przykładowo tryb T5 daje
w wyniku 361 odległości, a zatem 722 bajty. Po doliczeniu do tego pozostałych skład-
67
Praca ze skanerem laserowym SICK LMS200
ników przesyłanej ramki wychodzi łącznie 732 bajty. Aby mieć stuprocentową pewność
odnalezienia pełnej ramki należy odebrać conajmniej dwukrotnie tyle danych. Zamiana
uzyskanych odległości na postać dziesiętną polega na wykonaniu formuły:
d = bajtstarszy + 256bajtmodszy
Po znalezieniu nagłówka należy sprawdzić poprawność przesłanych danych poprzez
analizę sumy kontrolnej. Nie jest to postać standardowa zapisu tej sumy, lecz opracowana
przez firmę SICK. Poniższy kod może pomóc w implementacji tego zadania:
1 #define
Gen Polynomial
0 x8005
2
3 unsigned short u p d a t e c r c s i c k ( unsigned short c r c , char c ,
4 char p r e v b y t e )
5 {
6
unsigned short s h o r t c , s h o r t p ;
7
s h o r t c = 0 x 0 0 f f & ( unsigned short ) c ;
8
s h o r t p = ( 0 x 0 0 f f & ( unsigned short ) p r e v b y t e ) << 8 ;
9
10
11
i f ( c r c & 0 x8000 )
c r c = ( c r c << 1 ) ˆ Gen Polynomial ;
12
e l s e c r c = c r c << 1 ;
13
c r c &= 0 x f f f f ;
14
c r c ˆ= ( s h o r t c | s h o r t p ) ;
15
16
17 }
return c r c ;
Literatura
[BDW94]
Stephen Borthwick, Hugh Durrant-Whyte. Simultaneous localisation and map
building for autonomous guided vehicles. 1994.
[Com02]
Quick Manual for LMS communication setup, 2002.
[dar09a]
Darpa grand challenge. [on-line] http://www.darpa.mil/grandchallenge05, 2009.
[dar09b]
Darpa urban challenge. [on-line]
http://www.darpa.mil/grandchallenge/index.asp, 2009.
[GC96]
B. Dandrea-Novel G. Campion, G. Bastin. Structural properties and classification of
kinematic and dynamic models of wheeled mobile robots. 1996.
[Hol87]
K. Holejko. Precyzyjne elektroniczne pomiary odległości i kątów. Wydawnictwo
Naukowo Techniczne, 1987.
[JBF96]
H. R. Everett J. Borenstein, L. Feng. Where am I? Sensors and Methods for Mobile
Robot Positioning. The University of Michigan, Ann Arbor, MI 48109, 1996.
[Joe09]
User: Joe. Ulrasound. [on-line]
http://www.sensorwiki.org/doku.php/sensors/ultrasound, 2009.
[LDW91a] John J. Leonard, Hugh F. Durrant-Whyte. Mobile robot localization by tracking
geometric beacons. 1991.
[LDW91b] John J. Leonard, Hugh F. Durrant-Whyte. Simultaneous map building and
localization for an autonomous mobile robot. 1991.
[LMS06]
LMS200/211/221/291 Laser Measurement Systems, 2006.
[Paz02]
Dariusz Pazderski. Laboratorium podstaw robotyki. budowanie lokalnej mapy
otoczenia - skaner z czujnikiem podczerwieni. [on-line]
http://www.cse.put.poznan.pl, 2002.
[Paz09]
Dariusz Pazderski. Systemy lokalizacji i nawigacji robotów mobilnych. [on-line]
http://etacar.put.poznan.pl/dariusz.pazderski, 2009.
[RB09]
Søren Riisgaard, Morten Rufus Blas. Slam for dummies - a tutorial approach to
simultaneous localization and mapping. [on-line]
http://ocw.mit.edu/courses/aeronautics-and-astronautics/
16-412j-cognitive-robotics-spring-2005/projects/1aslam_blas_repo.pdf,
2009.
68
69
[sha09]
Sharp ir rangers information. [on-line]
http://www.acroname.com/robotics/info/articles/sharp/sharp.html, 2009.
[Skr07]
Piotr Skrzypczyński. Metody analizy i redukcji niepewności percepcji w systemie
nawigacji robota mobilnego. Poznań University of Technology, pl. M.
Skłodowskiej-Curie 2, 60-965 Poznań, 2007.
[VV04]
M. Veeck, W. Veeck. Learning polyline maps from range scan data acquired with
mobile robots. 2004.
[XZZ03]
[Zuf01]
Liu Jilin Xu Zehzong, Xiang Zhiyu. Scan matching based on cls relationships. 2003.
Jean-Christophe Zufferey. Application note for an infrared, triangultation-based
distance sensor with an analog, non-linear output. [on-line]
http://zuff.info/SharpGP2D12_E.html, 2001.
c 2010 Mateusz Przybyła
Katedra Sterowania i Inżynierii Systemów, Wydział Informatyki i Zarządzania
Politechnika Poznańska
Skład przy użyciu systemu LATEX.
BibTEX:
@mastersthesis{ key,
author = "Mateusz Przybyła",
title = "{Lokalizacja robota mobilnego i budowa mapy otoczenia z wykorzystaniem
skanera laserowego}",
school = "Poznan University of Technology",
address = "Pozna{\’n}, Poland",
year = "2010",
}

Politechnika Poznańska Wydział Informatyki i Zarządzania Katedra Sterowania i Inżynierii Systemów

Transkrypt

Podobne dokumenty