Dopasowanie map

Dopasowanie map odbywa si¦ poprzez sekwencyjne wywoªywanie algorytmu LevenbergaMarquardta. Kolejne iteracje wykonywane s¡ na mapach o coraz wi¦kszej rozdzielczo±ci, a wynik ka»dej iteracji (po odpowiednim przeskalo-waniu do wi¦kszej mapy) wykorzystywany jest jako warunek pocz¡tkowy iteracji kolejnej. W ten sposób otrzymuje si¦ kolejne  coraz dokªadniejsze

 przybli»enia wyniku dopasowania, a» do ostatecznego wyniku, b¦d¡cego przeksztaªceniem, które minimalizuje bª¡d wysoko±ci map w oryginalnej roz-dzielczo±ci. Pierwsze dopasowanie (dla map o najmniejszej rozdzielczo±ci) wykonywane jest z zerowymi warunkami pocz¡tkowymi  zakªada si¦, »e ro-bot si¦ nie przemie±ciª. Schemat blokowy algorytmu wyznaczaj¡cego kolejne przybli»enia wyniku przedstawiono na rysunku 7.4.

W ka»dej iteracji zaprezentowanego algorytmu, dla ka»dego stopnia szcze-góªowo±ci map, wykonywane jest dopasowanie wysoko±ci. Polega ono na wy-znaczeniu takiego przemieszczenia robota, które minimalizuje nast¦puj¡cy bª¡d e: W powy»szym równaniu zLi jest warto±ci¡ i-tego punktu z lokalnej mapy wysoko±ci (wysoko±ci¡ punktu Li), natomiast zGi jest odpowiadaj¡c¡ mu warto±ci¡ z globalnej mapy wysoko±ci, interpolowan¡ z 4 s¡siednich punk-tów. Wyra»enie gLi okre±la stopie« pofaªdowania mapy w punkcie Li i jest warto±ci¡ wyznaczan¡ dla lokalnej mapy wedªug wzoru (7.3). Dzi¦ki jego za-stosowaniu, wi¦ksz¡ wag¦ w wyznaczaniu bª¦du b¦d¡ miaªy punkty znajduj¡-ce si¦ na zboczach. Wielko±¢ uLi oznacza niepewno±¢ wysoko±ci wyznaczonej

Start

Wynik końcowy

Dopasowanie map p=16,

zerowy punkt startowy

zmniejszenie p, przeskalowanie

wyniku

p=1 ?

Tak Nie wynik jako punkt startowy

wynik

Rysunek 7.4: Schemat blokowy algorytmu wyznaczaj¡cego kolejne przybli»e-nia wyniku

dla i-tego punktu mapy lokalnej  warto±¢ ta jest odczytywana z odpowied-niej komórki lokalnej mapy niepewno±ci. Punkty o modpowied-niejszej niepewno±ci b¦d¡ miaªy wi¦ksz¡ wag¦ w wyznaczanym bª¦dzie. Wyra»enie oi okre±la wa-g¦ wynikaj¡c¡ z ró»nic w zaj¦to±ci mapy globalnej i lokalnej, i ma na celu uwzgl¦dnienie nowych obiektów, które mog¡ wyst¦powa¢ w nowym pomia-rze, a nie maj¡ swojej reprezentacji w mapie globalnej. W takim przypadku, punkty mapy lokalnej, odpowiadaj¡ce nowym obiektom nie b¦d¡ miaªy swo-ich odpowiedników w mapie globalnej i nie b¦d¡ mogªy zosta¢ poprawnie dopasowane. W zwi¡zku z tym, gdy punkt mapy lokalnej tra po przeksztaª-ceniu w miejsce, w którym nie ma »adnych pomiarów w mapie globalnej, jego wpªyw na bª¡d powinien by¢ niewielki. Je±li natomiast liczba pomiarów o_Li wchodz¡cych w skªad punktu Li jest taka sama jak liczba pomiarów oGi

wchodz¡cych w skªad punktu Gi, bª¡d powinien by¢ uwzgl¦dniany z peªn¡

wag¡. Podobnie jak w przypadku warto±ci zGi, warto±¢ zaj¦to±ci w punkcie G_i jest interpolowana z s¡siednich komórek globalnej mapy zaj¦to±ci. Waga

o_i obliczana jest nast¦puj¡co:

o_i = 0, 3 + r_oi· 0, 7, (7.5) gdzie roi okre±la ró»nic¦ w zaj¦to±ci punktu mapy lokalnej i odpowiadaj¡cego mu punktu mapy globalnej:

{

r_oi = 1− oLi/o_Gi, gdy oGi >= o_Li,

r_oi = 1− oGi/o_Li, w przeciwnym przypadku. (7.6) Nietrudno zauwa»y¢, »e warto±¢ ro b¦dzie zawsze zawiera¢ si¦ pomi¦dzy war-to±ciami 0 i 1.

Na rysunkach 7.57.6 przedstawiono wizualizacj¦ dziaªania opisywanego algorytmu. Na globaln¡ map¦ naniesiono czerwonymi okr¦gami punkty po-chodz¡ce z mapy lokalnej. Jak mo»na zauwa»y¢ na rys. 7.5 ab, dopasowanie z zerowymi warunkami pocz¡tkowymi map w oryginalnej rozdzielczo±ci ko«-czy si¦ utkni¦ciem algorytmu LevenbergaMarquardta w minimum lokalnym (rys. 7.5 b). Kolejne rysunki (7.5 c-e, 7.6) ukazuj¡ przemieszczanie warunków pocz¡tkowych w kierunku poprawnego wyniku.

7.2 Wyniki eksperymentalne

W celu sprawdzenia dziaªania algorytmu samolokalizacji przeprowadzono kil-ka eksperymentów. Na rysunku 7.8 przedstawiono mapy zbudowane podczas eksperymentu, w którym kamera oraz skaner laserowy zainstalowane zostaªy na robocie Messor, a ich poªo»enie skalibrowane zostaªo przy pomocy algo-rytmu przedstawionego w rozdziale 6.3.

Rysunek 7.8 b prezentuje map¦, do zbudowania której wykorzystano ªo»enie robota otrzymane z odometrii. Z uwagi na po±lizgi nóg robota, po-ªo»enie to wyra¹nie ró»niªo si¦ od rzeczywistego, co przejawia si¦ nieprawi-dªowym umieszczeniem nowych pomiarów w mapie. W przypadku wykorzy-stania proponowanego algorytmu mapa jest spójna, a elementy odpowiadaj¡

obserwowanym przeszkodom (rys. 7.8 c).

Rysunek 7.7 a przedstawia map¦ utworzon¡ w eksperymencie z senso-rem Kinect. Sensor zamontowany byª na podstawce wysoko±ci 35 cm i zostaª pochylony (wokóª osi x sensora) o k¡t 7^◦. Podstawka wraz z sensorem byªa nast¦pnie przesuwana r¦cznie wokóª makiety terenu, bez u»ycia jakiejkolwiek odometrii, w wyniku czego zarejestrowano 14 pomiarów. Na rysunku 7.7 b niebieskim kolorem oznaczono odtworzon¡ przy pomocy algorytmu samoloka-lizacji ±cie»k¦ (pierwszym pomiarem jest ten zlokalizowany w punkcie [0, 0]).

a b

c d

f e

Rysunek 7.5: Wizualizacja dziaªania algorytmu wyznaczaj¡cego kolejne przy-bli»enia wyniku  cz¦±¢ 1. a,b  próba dopasowania bez wykorzystania pre-zentowanej metody (odpowiednio przed i po optymalizacji); c,d  dopaso-wanie map pomniejszonych 16 razy; e,f  dopasodopaso-wanie map o rozdzielczo±ci

a b

c d

e f

Rysunek 7.6: Wizualizacja dziaªania algorytmu wyznaczaj¡cego kolejne przy-bli»enia wyniku  cz¦±¢ 2. a,b  dopasowanie map pomniejszonych 4 razy (od-powiednio przed i po optymalizacji); c,d  dopasowanie map pomniejszonych 2 razy; e,f  dopasowanie map w oryginalnej rozdzielczo±ci

a b

d c

Rysunek 7.7: Mapy utworzone z danych otrzymanych z sensora Kinect. Utwo-rzona mapa (a), odtwoUtwo-rzona ±cie»ka sensora (b), przykªadowe chmury punk-tów z 3 i 10 pomiaru (c), (d)

Czerwone linie odchodz¡ce od ±cie»ki oznaczaj¡ kierunek osi optycznej sen-sora w punktach, w których nast¦powaª pomiar.

Fragment utworzonej mapy, przedstawiaj¡cy makiet¦ nierównego tere-nu mo»na porówna¢ z map¡ ground truth, opisan¡ w rozdziaªach 5.3 i 6.4.

Wspomniany fragment miaª wymiary 121, 5 × 123 cm, a wi¦c nie odbiegaª wymiarami od makiety (o wymiarach 120 × 120 cm) o wi¦cej ni» 2, 5%, co wskazuje na poprawno±¢ zastosowanej metody samolokalizacji.

Wykonane eksperymenty pokazuj¡, »e przedstawiony algorytm dziaªa po-prawnie i pozwala na budow¦ rastrowych map otoczenia przy u»yciu sensorów zwracaj¡cych g¦sta map¦ gª¦bi.

a

b

c

Rysunek 7.8: Mapy utworzone z danych stereowizyjnych. Widok z kamery (a), mapa utworzona przy u»yciu samolokalizacji na podstawie odometrii (b), mapa utworzona przy u»yciu proponowanego algorytmu samolokalizacji (c)

Rozdziaª 8 Wnioski

8.1 Wnioski w kontek±cie dotychczasowego stanu wiedzy

W zagadnieniu generowania modelu sceny dla robotów krocz¡cych wci¡»

brakuje prostych i skutecznych rozwi¡za«, mog¡cych dziaªa¢ w czasie rze-czywistym. W niniejszej rozprawie podj¦to prób¦ uzupeªnienia tych braków.

Oprócz samego zagadnienia budowy modelu sceny, poruszono równie» zagad-nienia pokrewne, takie jak ltracja danych oraz kalibracja sensorów. Przed-stawiono tak»e propozycj¦ rozwi¡zania problemu samolokalizacji robota kro-cz¡cego po nierównym terenie  skuteczna samolokalizacja jest warunkiem koniecznym do integracji pomiarów dokonanych z ró»nych pozycji robota, a tym samym do budowy modelu bardziej rozlegªego ±rodowiska.

Podczas realizacji przedstawionych w rozprawie prac badawczych posªugi-wano si¦ kilkoma typami pasywnych i aktywnych sensorów wizyjnych. Wi¦k-szo±¢ z nich to produkty komercyjne, których konguracja i parametry nie mog¡ podlega¢ zmianom maj¡cym na celu dostosowanie ich do specycz-nych potrzeb robota krocz¡cego. Wyj¡tkiem jest aktywny system wizyjny z o±wietleniem strukturalnym. W rozdziale 3 zademonstrowano, »e kongu-racja takiego systemu mo»e zosta¢ zoptymalizowana z uwzgl¦dnieniem anali-tycznego opisu niepewno±ci pomiaru wysoko±ci prolu terenu oraz pewnych parametrów u»ytkowych robota krocz¡cego. Nast¦pnie przedstawiono sposób wykorzystania uzyskiwanych pomiarów do budowy rastrowej mapy

wysoko-±ci terenu. Zawarte w rozdziale 4 wyniki ilowysoko-±ciowe bada« systemu budowy mapy w kontrolowanych warunkach wskazuj¡, »e bª¦dy odwzorowania wyso-ko±ci obiektów w uzyskiwanej mapie nie przekraczaj¡ 5 mm, przy niewielkim odchyleniu standardowym pomiaru. Uzyskana precyzja pomiarów jest wy-starczaj¡ca do realizacji zadania wyboru punktów podparcia nóg robota

kro-cz¡cego, co potwierdzaj¡ wyniki przedstawione w powi¡zanych z tematyk¡

niniejszej rozprawy publikacjach [55][7].

Dysponuj¡c robotem krocz¡cym o wi¦kszym ud¹wigu mo»na jednak za-stosowa¢ do budowy mapy terenu ukªad wielosensoryczny. Przykªad takie-go ukªadu, zªo»onetakie-go ze skanera laserowetakie-go 2D oraz pasywnej kamery ste-reowizyjnej rozwa»ono w niniejszej pracy. Skaner laserowy umieszczony na pokªadzie robota w konguracji pochylonej zast¦puje ukªad z o±wietleniem strukturalnym, charakteryzuj¡c si¦ jednak wi¦kszym zasi¦giem oraz znacznie wi¦ksz¡ szybko±ci¡ akwizycji prolu terenu. Kamera stereowizyjna zapewnia natomiast mo»liwo±¢ postrzegania bardziej odlegªych obiektów i obszarów terenu. Jednoczesne wykorzystanie obu sensorów podczas ruchu robota wy-maga ich starannej wzajemnej kalibracji. Metod¦ kalibracji przedstawiono w rozdziale 6, podobnie jak sposób uwzgl¦dnienia w procesie aktualizacji rastrowej mapy wysoko±ci niepewno±ci pomiarów z obu sensorów. Zaprezen-towane w rozdziale 6 ilo±ciowe wyniki bada« eksperymentalnych z wykorzy-staniem makiety terenu o znanym ksztaªcie wskazuj¡, »e u»ycie danych ze skanera laserowego 2D istotnie poprawiªo jako±¢ odtworzenia geometrii nie-równego terenu w stosunku do mapy wysoko±ci wykorzystuj¡cej wyª¡cznie dane 3D, otrzymane z pasywnego sensora stereowizyjnego.

W aplikacjach autonomicznego robota mobilnego generowanie u»yteczne-go modelu sceny nie jest mo»liwe bez skutecznej samolokalizacji. W przy-padku robota krocz¡cego zadanie samolokalizacji jest w praktyce szczególnie trudne, mi¦dzy innymi ze wzgl¦du na konieczno±¢ u»ycia sensorów o ogra-niczonej dokªadno±ci pomiaru i szybko±ci akwizycji danych oraz z powodu trudno±ci w rozró»nianiu znacz¡cych cech sceny, gdy pole widzenia u»ytych sensorów ogranicza si¦ gªównie do terenu przed robotem. W niniejszej rozpra-wie podj¦to prób¦ opracowania metody samoloklizacji dostosowanej do po-trzeb i mo»liwo±ci autonomicznego robota krocz¡cego. Jej koncepcja opiera si¦ na u»yciu numerycznego algorytmu optymalizacji nieliniowej Levenberga-Marquardta oraz reprezentacji terenu w postaci mapy o stopniowo rosn¡cej rozdzielczo±ci. Wyniki zaprezentowanych w rozdziale 7 eksperymentów wska-zuj¡, »e metoda ta umo»liwia skuteczn¡ samolokalizacj¦ na podstawie nie-pewnych i niepeªnych danych z sensorów zwracaj¡cych g¦st¡ map¦ gª¦bi.

Wspóªczesna literatura dotycz¡ca zagadnie« percepcyjnych autonomicz-nych robotów mobilautonomicz-nych (np. [103][107]) wskazuje jednoznacznie na istotno±¢

uwzgl¦dnienia probabilistycznego charakteru pomiarów i reprezentacji oto-czenia robota. Dlatego podczas opisanych w niniejszej rozprawie prac badaw-czych zwrócono szczególn¡ uwag¦ na zagadnienia kalibracji sensorów i ukªa-dów wielosensorycznych oraz probabilistyczny opis niepewno±ci pomiarów.

W rozdziaªach 3, 5 i 6 zaprezentowano modele niepewno±ci u»ywane pod-czas bada« aktywnych i pasywnych sensorów wizyjnych. Modele te powstaªy

zgodnie z jednolit¡ koncepcj¡ propagacji niepewno±ci poprzez macierze kowa-riancji i aproksymacj¦ zale»no±ci nieliniowych, opart¡ na zastosowaniu jako-bianów. W przypadku braku mo»liwo±ci analitycznego opisu sposobu propa-gacji informacji w procesie przetwarzania danych pomiarowych zastosowano równowa»ne funkcjonalnie podej±cie Haralick'a [27].

Przedstawione w rozprawie algorytmy i metody przetwarzania danych z ró»nych sensorów wizyjnych oraz ukªadów wielosensorycznych realizuj¡

podstawowe bloki funkcjonalne zwi¡zane z generowaniem modelu sceny ro-bota krocz¡cego: wst¦pn¡ ltracj¦ danych, modelowanie niepewno±ci, budo-w¦ mapy wysoko±ci terenu oraz samolokalizacj¦. Pokazane rozwi¡zania mog¡

przetwarza¢ dane w czasie rzeczywistym, rozumianym jako czas dost¦pny po-mi¦dzy kolejnymi aktualizacjami danych wej±ciowych przez zyczne sensory.

Chocia» sterowanie robotem krocz¡cym nie nale»aªo do zagadnie« porusza-nych w niniejszej rozprawie, to praktyczna przydatno±¢ modelu sceny otrzy-mywanego opisywanymi tu metodami w zadaniach sterowania i nawigacji robota krocz¡cego zostaªa potwierdzona wynikami przedstawionymi w od-r¦bnych publikacjach [12][78][8][9]. Pozwala to stwierdzi¢, »e wykorzystanie wielomodowego systemu wizyjnego pozwala na generowanie w czasie rzeczy-wistym modelu sceny skutecznie wspieraj¡cego sterowanie autonomicznym robotem krocz¡cym.

Powy»sze wnioski ogólne, a tak»e szereg wniosków szczegóªowych przed-stawionych w poszczególnych rozdziaªach rozprawy pozwalaj¡ twierdzi¢, »e tezy sformuªowane w rozdziale 1.2 zostaªy dowiedzione.