AMBER. Automated Mapping of robot EnviRonment. Berecz Szabolcs Kovács Róbert Kúthy Előd Zoltán

AMBER Automated Mapping of roBot EnviRonment

Berecz Szabolcs

Kovács Róbert

Kúthy El˝od Zoltán

Budapesti M˝uszaki F˝oiskola Neumann János Informatikai F˝oiskolai Kar Intelligens Automatizált Rendszerek szakirány Konzulens: Vámossy Zoltán, f˝oiskolai docens

2004. november 8.

Tartalomjegyzék 1. A projekt célja

5

2. Irodalomkutatás 6 2.1. Képfeldolgozás, modellalkotás (Berecz Szabolcs, Kúthy El˝od Zoltán) . . . . . . . . . . . . . . . 6 2.1.1. Képjellemz˝ok kiemelése . . . . . . . . . . . . . . . . . . 7 2.1.1.1. A SUSAN módszer . . . . . . . . . . . . . . . 7 2.1.1.2. A Monogenic signal alapú módszer . . . . . . . 7 2.1.2. Felület-regisztráció . . . . . . . . . . . . . . . . . . . . . 8 2.1.2.1. A spin-image módszer . . . . . . . . . . . . . . 8 2.1.2.2. Az Iterative Closest Point algoritmus . . . . . . 9 2.1.2.3. A spin-image módszerre specializált ICP algoritmus . . . . . . . . . . . . . . . . . . . . . . 10 2.1.2.4. Legközelebbi pont meghatározása . . . . . . . . 10 2.1.2.5. Abszolút orientáció meghatározása . . . . . . . 12 2.1.2.5.1. Normális eloszlású zaj . . . . . . . . 12 2.1.2.5.2. Hibás pontpárosítások . . . . . . . . . 13 2.2. Navigáció (Kovács Róbert) . . . . . . . . . . . . . . . . . . . . . 13 2.2.1. A közelít˝o cellákra bontás módszerén alapuló útkeresési technikák . . . . . . . . . . . . . . . . . . . . . . . . . . 14 2.2.1.1. A quadtree algoritmus . . . . . . . . . . . . . . 15 2.2.1.2. A D* keresés . . . . . . . . . . . . . . . . . . . 15 2.2.1.3. A parti-game algoritmus . . . . . . . . . . . . . 16 2.2.1.4. Egyéb módszerek . . . . . . . . . . . . . . . . 17 3. Rendszerterv 3.1. A projekttagok feladatai . . . . . . . . . . . . . . . . . . . . . . 1

18 18

3.2. Képfeldolgozó modul (Kúthy El˝od Zoltán) . . . . . . . . . . . . 3.3. Modellkezel˝o modul (Berecz Szabolcs) . . . . . . . . . . . . . 3.3.1. Pozíció meghatározás . . . . . . . . . . . . . . . . . . . 3.3.2. Modellépítés . . . . . . . . . . . . . . . . . . . . . . . 3.3.2.1. Hibás adatok kezelése . . . . . . . . . . . . . 3.3.2.2. Mozgás érzékelése . . . . . . . . . . . . . . . 3.4. Irányítás modul (Kovács Róbert) . . . . . . . . . . . . . . . . . 3.4.1. A robot irányítása . . . . . . . . . . . . . . . . . . . . . 3.4.1.1. A generált modell hiányosságainak meghatározása . . . . . . . . . . . . . . . . . . . . . . 3.4.1.2. A következ˝o elérend˝o pozíció meghatározása . 3.4.1.3. Út generálása a választott pozícióhoz . . . . . 3.4.1.4. A hordozójárm˝u vezérlése . . . . . . . . . . . 3.4.2. A robot mozgásának kalibrálása . . . . . . . . . . . . . 3.5. Tesztelés . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4. Megvalósítás 4.1. Képfeldolgozó modul (Kúthy El˝od Zoltán) . . . . . . . . 4.1.1. Kamerakezelés . . . . . . . . . . . . . . . . . . 4.1.2. Kamerakalibráció . . . . . . . . . . . . . . . . . 4.1.3. A megfelel˝o pontok kiválasztása . . . . . . . . . 4.1.4. Pontpárok képzése . . . . . . . . . . . . . . . . 4.1.5. A mélység meghatározása . . . . . . . . . . . . 4.2. Modellkezel˝o modul (Berecz Szabolcs) . . . . . . . . . 4.2.1. Modellek ábrázolása . . . . . . . . . . . . . . . 4.2.1.1. A mesh osztály . . . . . . . . . . . . . 4.2.1.2. A vertex és face osztályok . . . . . . . 4.2.1.3. A breadth_first_vertex_iterator osztály 4.2.1.4. A marker és markable osztályok . . . 4.2.1.5. A indexed_v3d osztály . . . . . . . . . 4.2.2. A spin_stack osztály . . . . . . . . . . . . . . . 4.2.2.1. Teszteredmények . . . . . . . . . . . 4.2.3. A transzformációk ábrázolása . . . . . . . . . . 4.3. Irányítás modul (Kovács Róbert) . . . . . . . . . . . . . 4.3.1. A modell térképpé konvertálása . . . . . . . . . 2

. . . . . . . .

18 19 19 20 20 21 21 21

. 21 . 22 . 22 . 23 . 23 . 24

25 . . . . . 25 . . . . . 25 . . . . . 25 . . . . . 27 . . . . . 28 . . . . . 29 . . . . . 30 . . . . . 31 . . . . . 32 . . . . . 32 . . . . . 32 . . . . . 33 . . . . . 34 . . . . . 34 . . . . . 34 . . . . . 36 . . . . . 37 . . . . . 37

4.3.2. Az út generálása – A parti-game algoritmus . . . . . . . . 4.3.2.1. Inicializáció . . . . . . . . . . . . . . . . . . . 4.3.2.2. A f˝o eljárás . . . . . . . . . . . . . . . . . . . 4.3.2.3. A cellák céltól való távolságának meghatározása 4.3.2.4. Tesztek . . . . . . . . . . . . . . . . . . . . . . 4.3.3. A generált út átalakítása . . . . . . . . . . . . . . . . . . 5. Eredmények, célkituzések ˝

38 39 40 41 42 43 48

3

Ábrák jegyzéke 2.1. Egy spin-image által hordozott információ . . . . . . . . . . . . . 2.2. Kd-tree felosztás két dimenzió esetén . . . . . . . . . . . . . . .

9 11

3.1. Párhuzamos optikai tengelyek . . . . . . . . . . . . . . . . . . . 3.2. Összetartó optikai tengelyek . . . . . . . . . . . . . . . . . . . . 3.3. Föltérképezett terület . . . . . . . . . . . . . . . . . . . . . . . .

19 19 22

4.1. 4.2. 4.3. 4.4. 4.5. 4.6. 4.7. 4.8. 4.9. 4.10. 4.11. 4.12. 4.13.

26 28 30 30 34 35 36 43 43 43 44 45 46

A kamerakalibráció eredményeként kapott görbék . . Token-detektálás különböz˝o paraméter értékek esetén A pontpárok ábrázolása a két kép átlagán . . . . . . Megtalált pontpárok összekötve . . . . . . . . . . . A „szoba” modellje . . . . . . . . . . . . . . . . . . Kacsa modell, egy és két százalék zaj . . . . . . . . „Szoba” modell, egy és két százalék zaj . . . . . . . Várhatóan a valódihoz hasonló térkép . . . . . . . . Nagy bonyolultságú térkép . . . . . . . . . . . . . . Sz˝uk folyosókat tartalmazó térkép . . . . . . . . . . Vontató és vontatandó összekapcsolása . . . . . . . . Átalakított út . . . . . . . . . . . . . . . . . . . . . Az algoritmus hibájából adódó ütközések . . . . . .

4

. . . . . . . . . . . . .

. . . . . . . . . . . . .

. . . . . . . . . . . . .

. . . . . . . . . . . . .

. . . . . . . . . . . . .

. . . . . . . . . . . . .

. . . . . . . . . . . . .

1. fejezet A projekt célja A projekt célja olyan rendszer megtervezése és elkészítése, mely két kamerából nyert információ alapján a környezetér˝ol egy háromdimenziós modellt készít. Mivel ennek felépítéséhez szükséges, hogy a kamera több helyr˝ol legyen képes felvételeket készíteni, a rendszernek rendelkeznie kell egy minél több szabadsági fokú hordozóeszközzel, melyet irányítania kell. A feladat megoldása három f˝o részb˝ol áll, a kamerák által közvetített képekb˝ol a környezeti információk kinyerése, ezek alapján a környezet modelljének megalkotása, majd a modell alapján a robot irányítása.

5

2. fejezet Irodalomkutatás 2.1.

Képfeldolgozás, modellalkotás (Berecz Szabolcs, Kúthy El˝od Zoltán)

Mivel a következ˝okben felvázolt terület nem bontható két – közel azonos nehézség˝u – részre, úgy döntöttünk, hogy egymást kiegészítve ismerkedünk meg a mások által használt algoritmusokkal, és közösen írjuk meg az általunk használható algoritmusok áttekintését. Megismerkedtünk az alapvet˝o képjellemz˝ok kiemelésének tipikus módszereivel (élkiemelés), mélységinformációk kinyerésével két kép különbségeinek alapján, illetve két ponthalmaz összeillesztésével[3]. Az élkiemel˝o algoritmusok három osztályra bonthatóak: a gradiens és Laplace operátort közelít˝o módszerek, a template matching módszert alkalmazó algoritmusok, és a parametrikus élmodellekkel m˝uköd˝o algoritmusok. A gradiens operátort közelít˝o algoritmusok (Robert’s cross operator, Söbel, Prewitt, Kirsch, Robinson) a differenciáló természetük miatt a zajra nagyon érzékenyek, ami fokozottan jellemz˝o a Laplace operátort alkalmazó algoritmusokra, ráadásul az el˝oz˝o módszerekkel ellentétben az intenzitásváltozás irányát sem adják meg. A template matching módszert alkalmazó algoritmusok csak el˝ore meghatározott struktúrájú élek nagy biztonságú meghatározására alkalmasak. A parametrikus modelleket használó algoritmusok részletesebb élstruktúrák meghatározására alkalmasak, de számításigénye az el˝oz˝oekben felvázolt módszereknél jelent˝osen nagyobb. Mélységinformációk kinyerésére két módszert ismertet Ballard és Brown[3], melyek a terület alapú, és a képjellemz˝oket felhasználó eljárások. A terület alapú módszerek alapfeltevése az, hogy a két kép közötti különbség minimális – ezért csak kis kamera bázistávolság esetén alkalmazható –, és a hasonló területek képen elfoglalt helyzete alapján számítja a távolságot. Ezen algoritmusok egy kissé 6

pontatlan, s˝ur˝u mélységtérképet eredményeznek, melyek további feldolgozást igényelnek a számunkra hasznos információk kinyeréséhez (egy mesh felépítéséhez). A képjellemz˝ok alapján m˝uköd˝o algoritmusok ritka mélységtérképet állítanak el˝o, melyek lokálisan pontosabbak, és globálisan megbízhatóbbak, mint a terület alapú módszerek eredménye, valamint a már meghatározott képjellemz˝ok felhasználhatóak temporal matching eljárások bemeneteként, melyek tovább pontosíthatják a pontok helyzetének meghatározását. Megoldásként a két ponthalmaz összeillesztésének problémájára a gráf izomorfizmusok meghatározását mutatja be. Ezen gráf izomorfizmusok meghatározása az NP-teljes problémák csoportjába tartozik, ezért általunk nem hasznosítható. A további irodalomkutatás során újabb algoritmusokat ismertünk meg, melyek közül az általunk használhatóak áttekintése következik.

2.1.1. Képjellemz˝ok kiemelése A következ˝o algoritmusok közös jellemz˝oje, hogy nem csak éldetektálást végeznek, hanem a kép néhány jellegzetes pontjának (pl. élcsomópontok) kiemelésére is használhatóak, melyekkel egyszer˝ubben megoldható a képjellemz˝ok párosítása a mélységinformációk kinyerése érdekében. 2.1.1.1.

A SUSAN módszer

Ez az algoritmus[20] integráló jelleg˝u, ezért kevésbé érzékeny a zajra, valamint pontosabban meghatározza a csomópontok helyzetét, mint a gradiens alapú módszerek. A módszer elméletileg is jól alátámasztott, és csak egy empirikus paramétert használ, amely a zajsz˝urés mértékét határozza meg. 2.1.1.2.

A Monogenic signal alapú módszer

Az egydimenziós jelfeldolgozásban jól ismert módszer az analitikus jel, mely komplex számsíkon ábrázolja az adott jelet. Alapvet˝o tulajdonsága a jellemz˝ok felosztása, polár koordinátarendszerben a komplex jel kitev˝oje és alapja szerint különíthet˝oek el a jel jellemz˝oi. A kitev˝o a jel lokális kvantitatív jellemz˝oit írja le, melyre a lokális amplitúdó elnevezés használatos. Az alap pedig a jel lokális kvalitatív jellemz˝oit írja le, amit lokális fázisnak hívunk. A lokális amplitúdó és fázis tulajdonsága az invariancia és az ekvivariancia, azaz a lokális amplitúdó független a lokális struktúrára, de leírja a lokális energiát. A lokális fázisra pedig pont a fordítottja igaz. Az analitikus jel a komplex számsíkon alkalmazott Hilbert transzformáció segítségével állítható el˝o. 7

Sajnos ez a leírás közvetlenül nem alkalmazható kétdimenziós jelekre, mivel nem rendelkezik kell˝o szabadsági fokkal, vagyis a módszer kiterjesztésekor a jel elveszti az invariancia és ekvivalencia tulajdonságait, így egy rendszeres hiba keletkezik, mely a jel irányától függ. Ezen probléma feloldására Felsberg egy harmadik komponens bevezetését javasolta, a lokális orientációt, mely a jel geometriai jellemz˝oit írja le. Az analitikus jel ezen kiterjesztését nevezzük monogenikus jelnek. Ezen jel el˝oállításához a kvaterniókat alkalmazó Reisz transzformáció használatos. Mivel ez a módszer a kép jellegzetes pontjait számos paraméterrel jellemzi (irány, fázis, két meghatározó szín, energia), melyek nagymértékben pontosítják két jellegzetes pont párosítását a képeken[6][11].

2.1.2. Felület-regisztráció A pontok kameráktól mért távolságának meghatározása után következ˝o lépés a már meglév˝o modellhez viszonyított kamerahelyzet meghatározása. Ennek megvalósítására két – általunk használható – algoritmust találtunk, mindkett˝o magába foglalja az abszolút orientáció, valamint a legközelebbi pont meghatározásának problémáját. 2.1.2.1.

A spin-image módszer

Az algoritmus[10] azokban az esetekben alkalmazható, amikor a regisztrálandó ponthalmaz minden pontjához orientáció számítható. Egy felület egyértelm˝uen kielégíti ezt a feltételt, ugyanis a felület egy adott pontjához tartozó normálvektor – amennyiben úgy határozzuk meg, hogy a pontból a kamera felé mutató vektorral 0◦ ≤ α ≤ 90◦ szöget zár be – egy egyértelm˝u irányt határoz meg. A regisztráció elvégzéséhez létre kell hoznunk a modell spin-image reprezentációját, ami a felület minden pontjához egy kétdimenziós tömböt (képet) rendel, mely megmutatja, hogy az adott pont és vektor által meghatározott egyenest˝ol (x koordináta) és síktól (y koordináta) milyen távolságra vannak a felület pontjai (2.1. ábra). Ez a reprezentáció független az eltolástól és forgatástól, így a regisztrációhoz nincs szükség egy kezdeti, kell˝oen pontos transzformációra. A modell spin-image-einek létrehozása után a regisztrálandó felületb˝ol véletlenszer˝uen kiválasztott néhány pontnak is elkészítjük a spin-image reprezentációját, majd ezek és a modell spin-image-einek korrelációját kiszámítjuk. A hasonló spin-image-ek alapján létrehozunk pont-megfeleltetéseket a modell és a regisztrálandó ponthalmaz között. Mivel hibás megfeleltetéseket is kaphatunk, érdemes csoportosítanunk: egy csoportba egymástól távol es˝o pont-megfeleltetéseket teszünk. A kapott csoportok mindegyike alapján meghatározunk egy transzformációt (lásd 2.1.2.5. pont), 8

ezt pontosítjuk a spin-image módszerre specializált ICP algoritmussal (2.1.2.3. pont), majd kiválasztjuk azt, amellyel elvégezve a transzformációt a legtöbb pontegyezést találjuk a két ponthalmaz között (másképpen: hány olyan pontpárt találunk, melyek távolsága egy bizonyos értéknél kisebb). Azzal, hogy távoli pontokat helyezünk egy csoportba azt is biztosítjuk, hogy a pontok pozíciójában jelen lév˝o zajnak kis hatása legyen a kiszámított transzformációra (annál nagyobb a transzformáció hibája, minél nagyobb a hibák összegének és a pontok távolságának hányadosa).

2.1. ábra. Egy spin-image által hordozott információ (forrás: Johnson[10])

2.1.2.2.

Az Iterative Closest Point algoritmus

Az ICP algoritmus[4] iterált jelleg˝u, és három alapvet˝o lépésb˝ol áll: 1. Megkeressük a regisztrálandó ponthalmaz egyes pontjaihoz legközelebb es˝o pontokat a modell ponthalmazban (ld. 2.1.2.4. pont). 2. Transzformáció meghatározása, amely minimalizálja a pontpárok közötti távolság négyzetét (ld. 2.1.2.5. pont). 3. A regisztrálandó ponthalmazon elvégezzük az el˝oz˝o lépésben meghatározott transzformációt. A módszer leggyengébb pontja az els˝o lépés, melynek célja az, hogy a regisztrálandó és a modell ponthalmaz pontjai között létrehozzon egy megfeleltetést. Ez a megfeleltetés csak abban az esetben lesz kielégít˝oen jó, ha ismerünk egy megközelít˝oleg helyes kiindulási transzformációt. Ha ez a transzformáció nagy eltérést 9

mutat a tényleges transzformációhoz képest, akkor nagy valószín˝uséggel lokális minimumhoz közelít az algoritmus, ezért néhány iteráció után – mikor már csak kismértékben változik a második lépés során kiszámított transzformáció – érdemes ellen˝oriznünk, hogy megfelel˝o-e a kapott transzformáció: összeszámoljuk, hogy a regisztrálandó ponthalmazban lév˝o pontok hány százalékához találunk egy el˝ore meghatározott távolságnál közelebb lév˝o pontot a modell ponthalmazban. Ha ez az érték kisebb, mint egy el˝ore meghatározott küszöb, akkor a transzformációt hibásnak tekintjük. Ha a pontokhoz a pozíción kívül más tulajdonságokat is rendelhetünk (pl. felületi normálvektor, szín), akkor ezt is felhasználhatjuk az els˝o lépésben, így pontosabb összerendeléseket kaphatunk. Ebben az esetben több mint három dimenziót kell használnunk a legközelebb es˝o pont meghatározásakor: minden tulajdonsághoz egy-egy dimenzió – például pozíció és normálvektor esetén 3+3 dimenzió. Ha különböz˝o mérték˝u maximális eltérést engedélyezünk az egyes tulajdonságokra, akkor a távolságszámítás során ennek megfelel˝oen súlyoznunk kell az összetev˝oket. Az iteráció leállási feltételét meghatározhatjuk az iterációk maximális számával, a megengedett legnagyobb hibával, vagy a konvergencia sebességével. 2.1.2.3.

A spin-image módszerre specializált ICP algoritmus

Az egyetlen különbség az általános ICP-hez képest a pontpárok meghatározásában van. Mivel a spin-image algoritmus már meghatározott néhány pontpárt, így érdemes ezekb˝ol kiindulni: az egyes pontpároktól elindulva, azoktól egyre távolodva végighaladunk a regisztrálandó felület egyes pontjain, és megvizsgáljuk, hogy találunk-e ehhez egy el˝ore meghatározott távolságnál közelebb lév˝o pontpárt a modell ponthalmazban. Ha találtunk, akkor feljegyezzük ezt a pontpárt, és továbbhaladunk a felületen. Ha nem találtunk, akkor ezt az irányt zsákutcának tekintve visszafordulunk. Ezzel a módszerrel a kiindulási pontpárok környezetében terjesztjük szét a pont-megfeleltetéseket, ezáltal kisz˝urve a hibás pontpárokat, melyeket egy egyszer˝u, minden ponton végighaladó kereséssel találtunk volna. 2.1.2.4.

Legközelebbi pont meghatározása

A triviális megoldásnál (minden pontra meghatározzuk az adott koordinátától számított távolságot) hatékonyabban határozhatjuk meg a legközelebbi pontot a Kd-tree adatstruktúrával. A Kd-tree a Binary Search Tree általánosításaként is felfogható, ami a BST egy kulcsával szemben k darab kulcs alapján keres (k a dimenziók száma). Az adatstruktúra a ponthalmazt a fa egyes szintjein egy-egy 10

2.2. ábra. Kd-tree felosztás két dimenzió esetén

dimenzió alapján osztja két részre (2.2. ábra). Annak meghatározására, hogy az egyes szinteken melyik dimenziót használjuk több módszert is alkalmaznak: – A legegyszer˝ubb megoldás, ha az egyes dimenziókat sorrendben választjuk (a fa i-edik szintjén a (i mod k)-adik dimenziót) – Optimálisabb struktúrát eredményez, ha a ponthalmaz vizsgálata alapján határozzuk meg a szétválasztó dimenziót. Egy ilyen struktúra felépítése számításigényesebb, és pontok módosításakor/törlésekor is érdemes figyelembe venni az optimalitást, ezert csak kismértékben változó adatok esetén érdemes használnunk. A szétválasztás pozíciójának meghatározására valószín˝uleg az a legjobb módszer, ha a ponthalmaz adott dimenzió szerinti mediánjának koordinátáját választjuk, így kismértékben változó ponthalmaz esetén gyakorlatilag O(log n) id˝o alatt kereshetünk a struktúrában. Gyorsan változó ponthalmaz esetén annak érdekében, hogy hasonló hatékonysággal kereshessünk, folyamatosan az optimalitás közelében kell tartanunk a struktúrát. Ezt elérhetjük úgy, hogy a módosítások során optimális állapotban tartjuk a struktúrát, vagy id˝onként egy teljes optimalizációt hajtunk végre. A Kd-tree adatstruktúra segítségével viszonylag gyorsan kaphatjuk meg egy adott koordinátától adott távolságon belül lév˝o pontokat. Ezt használhatjuk fel a legközelebbi pont meghatározásához: egy – az alkalmazástól függ˝o – maximális távolságot meghatározva keresünk a struktúrában, majd az így kiválasztott pontok közül lineáris kereséssel választjuk ki a legközelebbi pontot. Ezzel a módszerrel kezelhetjük a pontatlan adatokat is, ebben az esetben a várható maximális bizonytalanság kétszeresével meg kell növelnünk a megengedett maximális távolságot, majd egy utólagos sz˝urés során eldobjuk a keresett tartományon kívül es˝o pontokat.

11

2.1.2.5.

Abszolút orientáció meghatározása

Az abszolút orientáció probléma megoldásán két ponthalmaz közötti transzformáció meghatározását értjük. A keresett transzformáció meghatározására két megközelítés t˝unik megfelel˝onek: a Horn[8][9] által kidolgozott módszer normális eloszlású zaj esetén ad optimális eredményt; Micheals[15] módszere pedig abban az esetben alkalmazható eredményesen, ha a ponthalmazok közötti megfeleltetések nem tökéletesek. 2.1.2.5.1. Normális eloszlású zaj [8][9]. Horn a megoldás levezetésekor abból indult ki, hogy a legmegfelel˝obb eredményt a n X i=1

wi kr r,i − sM (θ, u)r l,i − r 0 k2

minimalizálásával kapjuk (ahol n a pontok száma; wi az i-edik pont-pozíció pontosságának mér˝oszáma; r l,i a „bal”, r r,i a „jobb” ponthalmaz i-edik pontjának a ponthalmaz tömegközéppontjához viszonyított helyzete; s a skálázás mértéke; M (θ, u) az u vektor által meghatározott tengely körüli θ mérték˝u forgatást leíró mátrix). A keresett értékek: s, θ, u, r 0 . Ebb˝ol a kifejezésb˝ol látszik, hogy minden pontot figyelembe vesz – esetleg különböz˝o súllyal – a transzformáció kiszámításához, így nem kezeli a hibás pontpárosításokat. Horn levezetéséb˝ol kiderül, hogy a legmegfelel˝obb eltolást a r 0 = r r − sM (θ, u)r l képlettel kaphatjuk meg (r l , r r a „bal”, illetve a „jobb” ponthalmaz tömegközéppontja), tehát egyszer˝uen kiszámítható, ha már ismerjük a szükséges forgatást valamint skálázást. Mivel esetünkben nincs szükség skálázásra, ezt konstans egynek véve már csak a forgatás meghatározása van hátra. Ehhez a pontok koordinátáit felhasználva egy 4 × 4-es mátrixot építünk, melynek segítségével megkaphatjuk a keresett forgatást. A mátrix felépítéséhez el˝oször kiszámítjuk az Sxx , Sxy , Sxz , Syx , Syy , Syz , Szx , Szy , Szz értékeket, ahol Sxx =

n X

wi xl,i xr,i

Sxy =

n X

wi xl,i yr,i

...

i=1

i=1

(xl,i és yr,i a megfelel˝o pontok x illetve y koordinátái), majd ezen értékek összegeib˝ol és különbségeib˝ol felépítjük a 4 × 4-es mátrixot: Sxx + Syy + Szz Syz − Szy  N = Szx − Sxz Sxy − Syx 

Syz − Szy Sxx − Syy − Szz Sxy + Syx Szx + Sxz

Szx − Sxz Sxy + Syx −Sxx + Syy − Szz Syz + Szy

 Sxy − Syx Szx + Sxz   Syz + Szy −Sxx − Syy + Szz

Az N mátrix legpozitívabb sajátértékéhez tartozó sajátvektorát kvaternióként értelmezve megkapjuk a keresett forgatást. 12

2.1.2.5.2. Hibás pontpárosítások[15]. Micheals módszere egy olyan – Hornétól különböz˝o – zárt formájú megoldáson alapszik, mely 4 pontpár alapján számítja ki a szükséges transzformációt. A megoldás azt aknázza ki, hogy a forgatást reprezentáló kvaternió komponenseinek páronkénti szorzatát kifejezhetjük a pontok lineáris kombinációival, tehát a megoldáshoz egy lineáris egyenletrendszert kell megoldanunk (a konkrét egyenletekre a méretei miatt itt nem térek ki). A megoldás során a qsx , qsy , qsz , qxy , qxz , qyz , qs2 , qx2 qy2 , qz2 szorzatok értékeit számítjuk ki (qsx = qs qx , qx2 = qx qx , . . . ). Micheals megfigyelései szerint a komponensek négyzete kevésbé érzékeny a zajra, mint a többi komponens-szorzat. A számítások rövidebb id˝o alatt elvégezhet˝ok mint a sajátérték/sajátvektor számítás, viszont csak 4 pontpár közötti transzformáció meghatározására használható. Ha több pont áll rendelkezésre, alkalmazhatunk egy RANSAC[7]-hoz hasonló megközelítést: a ponthalmaz pontjait sorrendben bejárva az egymás utáni pont-négyesekre elvégezzük a számítást, majd a részeredményeket – azok „jósága” szerint súlyozva – átlagoljuk. Az egyes részeredmények súlyának számításához felhasználhatjuk az egyes pontok, és ezek transzformált párjainak páronkénti távolságának négyzetösszegét (d érték), valamint – a különböz˝o zajérzékenység miatt – a kvaternió-komponensek szorzatait: 2 2 2 2 2 2 p = |qs2 qx2 −qsx |+|qs2 qy2 −qsy |+|qs2 qz2 −qsz |+|qx2 qy2 −qxy |+|qx2 qz2 −qxz |+|qy2 qz2 −qyz |

2 (zajt tartalmazó adatok esetén a qs2 qx2 = qsx egyenl˝oség – a zajt nem tartalmazó esettel ellentétben – nem teljesül). Nagy d és p érték esetén a részeredmény a súlyozás miatt csak kismértékben – esetleg egyáltalán nem – változtat a végeredményen.

2.2.

Navigáció (Kovács Róbert)

A különleges esetekt˝ol eltekintve a robot környezetének teljes föltérképezéséhez szükséges a kamera célzott mozgatása, ezt hardveres megvalósítás esetén egy modellautó segítségével tervezzük megoldani. Szükséges, hogy a robot meg tudja határozni a jelenlegi helyzetét, esetleges elmozdulását az el˝oz˝o helyzetéhez képest, az elérend˝o pozíció(ka)t és a célhoz vezet˝o utat. A célhoz vezet˝o út meghatározására két, ugyanazon alaptechnikán alapuló módszert találtam megfontolásra méltónak: az egyik az un. pontos cellákra bontás (exact cell decomposition), a másik a közelít˝o cellákra bontás (approximate cell decomposition)[12]. Mindkét módszer lényege, hogy a robot környezetét egymást nem fed˝o régiókra – cellákra – bontjuk, majd a szerint, hogy a cella szabad teret 13

vagy tereptárgyat tartalmaz, osztályozzuk azokat. A különbség a két módszer között, hogy az els˝o esetben a cellák formája nem meghatározott, így lehet˝oség van olyan felbontásra, hogy kizárólag kétféle cella képz˝odjön: olyan, amely területén nincs tereptárgy és olyan, amelynek területe csak tereptárgyat tartalmaz (olyan sokszögeket képezünk, amelyek határai az egyes tereptárgyak csúcsaihoz és éleihez igazodnak). Második esetben el˝ore meghatározott méret˝u és formájú cellákra bontjuk a környezetet, így általában létrejön egy vegyes cellatípus is, amelynek területén szabad tér és tereptárgy is található. Ezek után létrehozzuk a kapcsolat gráfot (connectivity graph), mely az egyes cellák közötti szomszédságot jelöli oly módon, hogy a cellákat a gráf csúcsai jelölik és két csúcs között akkor és csak akkor van él, ha a megfelel˝o két cella szomszédos egymással, vagyis a celláknak van közös határolószakaszuk. Következ˝o lépésként megpróbálunk e gráfban valamilyen módszerrel egy utat keresni a kiindulási és a célcella között, vagy csak üres, vagy üres és vegyes cellákon végighaladva. A pontos cellákra bontás el˝onye, hogy a gráf felépítése után egyszer˝u gráf kereséssé válik a probléma, hátránya viszont, hogy a gráf elkészítésének bonyolultsága több nagyságrendet változhat a környezet komplexitásától függ˝oen. A közelít˝o cellákra bontás el˝onye, hogy az állapotteret egyszer˝u módszerrel, hatékonyan lehet fölbontani cellákra, az elkészített gráf mérete könnyen becsülhet˝o, az el˝oz˝o megközelítéshez képest ezen algoritmus általában gyorsabb, egyetlen hátránya – a vegyes cellák okozta esetleges problémák – például változó cellamérettel orvosolható.

2.2.1. A közelít˝o cellákra bontás módszerén alapuló útkeresési technikák Ezeknél a módszereknél lényeges, hogy mekkora cellákkal dolgozunk: ha túl nagyok, akkor sok utat hagyunk figyelmen kívül, túl kicsi esetén pedig nagyon számításigényessé válik az eljárás. Általánosan alkalmazott megoldás a problémára, hogy kiválasztunk egy kiindulási cellaméretet, felépítjük az ennek megfelel˝o kapcsolódási gráfot, és ha ebben a gráfban nem sikerült utat találni, akkor a valamilyen heurisztika szerint kiválasztott cellákat felosztjuk kisebb részekre, beillesztjük a gráfba és az új gráfban is megpróbálunk utat találni. Az eljárást addig folytatjuk, amíg el nem érjük a célt vagy valamilyen megállási feltételt. Többféle eljárás alapul a fenti alapelven, ebb˝ol kett˝ot találtam megfontolásra méltónak, mivel mindkett˝o alkalmazható dinamikusan változó térképekre is – amely a mi esetünkben alapfeltétel: A quadtree fölbontáson (quadtree decomposition) alapuló D* keresés[21] és a parti-game algoritmus[16][17].

14

2.2.1.1.

A quadtree algoritmus

A quadtree algoritmus a robot környezetét háromféle cellára bontja a szerint, hogy a cellák mekkora részét foglalja el valamilyen tereptárgy: egy cella üres, ha egyáltalán nincs benne tereptárgy, vegyes, ha egy részében van, és teli, ha a cella egy tárgy belsejében fekszik. Amennyiben létezik az elkészített kapcsolódási gráfban olyan út a célig, amely csak üres cellákat tartalmaz, úgy az algoritmus ezt az utat használja. Ha nem létezik, akkor ezeket a cellákat további négy cellára bontja föl, majd ezeket is beilleszti a kapcsolati gráfba és ezen a gráfon hajt végre egy keresést. Ha még így sem talált üres cellákból álló utat, akkor tovább bontja a cellákat egészen egy minimum mértékig. 2.2.1.2.

A D* keresés

A D* keresés nulladik lépéseként meg kell határozni az indulási és a cél cellákat a gráfban. Minden cellának – kivéve a célcellát – van egy mutatója amely a cél eléréséhez vezet˝o úton a következ˝o cellára mutat, van egy tulajdonsága, amely NYÍLT, ZÁRT és ÚJ értékeket vehet fel, létezik továbbá minden szomszédos cella között egy költség, amely a cellák közti áthaladás költségét reprezentálja. Az algoritmus karban tart egy NYÍLT listát, amely arra szolgál, hogy az állapottérben bekövetkezett változásokat tovaterjessze a cellák között. Egy cella NYÍLT tulajdonságú, ha ezen a listán van, ZÁRT, ha már kikerült bel˝ole és ÚJ, ha még soha nem volt rajta. Az eljárás minden cellára nyilvántart még egy céltól való becsült távolságot és egy kulcs tulajdonságot, amely a módosítás utáni céltól való távolságok és a módosítás el˝otti céltól való távolság minimuma. A NYÍLT listán lév˝o cellák e kulcs szerint vannak növekv˝oen rendezve. A kulcs funkció lehet˝ové tesz tovább egy osztályozást a szerint, hogy a listán lév˝o cella céltól való távolsága egyenl˝ore ezzel az értékkel – ebben az esetben LOWER, egyébként RAISE besorolást kap. Els˝o lépésként minden cella ÚJ tulajdonságot kap, a célcella céltól való távolságát nullára állítjuk majd ráhelyezzük a NYÍLT listára. Ezek után levesszük a listáról az els˝o elemet – itt a cél cellát – és megvizsgáljuk a szomszédos elemek céltól való távolságát, LOWER és RAISE tulajdonságait, és ezek alapján döntünk arról, hogy a szomszédos cella fölkerüljön-e a listára – az kiválasztó algoritmus részletesebb leírása megtalálható a [21]-ben. Az eljárást addig folytatjuk, amíg a kiindulási cella át nem megy a nyílt listán. A környezetben bekövetkezett változás esetén az algoritmus a NYÍLT listára teszi az érintett cellákat és így végzi el a fent vázolt eljárást. Lényeges, hogy a kiválasztási metódus és a megállási feltétel miatt csak azon cellák adatai kerülnek egy változáskor frissítésre, amelyek frissítése esetén esély van egy rövidebb 15

út felfedezésére. Az algoritmus így lényegesen gyorsabb, mintha – az egyébként kiindulásként használt – A* algoritmust alkalmaznánk – mivel az minden változáskor igényli a teljes újraszámolást – ugyanakkor funkcionálisan ekvivalens azzal. A változások esetén történ˝o frissítések további lokalizálására Stentz[22] bevezette a Focussed D* algoritmust, amely további sebességnövekedést ígér azokban az esetekben, amikor a környezetfrissítések mennyisége nem halad meg egy bizonyos mértéket. 2.2.1.3.

A parti-game algoritmus

A parti-game algoritmus az állapotteret négyszöglet˝u cellákra bontja, majd egy virtuális ügynököt a startpozícióból elindítva megpróbál a célcellába jutni, közben esetlegesen tovább finomítva a cellákra bontást. A cellák költségének meghatározásához egy játékelméleti megközelítést alkalmaz. Az ágens minden pillanatban meg tudja állapítani, hogy mely pozícióban és mely cellában van. Minden cellából lehet˝osége van az összes szomszédos cellába továbbhaladni, megcélozva a cellák közepét. Minden cella nyilvántartja az ágens mozgásának eredményeit, ezeket hármasokban eltárolja a tapasztalati adatbázisba, mely hármas els˝o eleme, hogy mely cellából indult az ügynök, második, hogy melyik cellát célozta meg, harmadik pedig, hogy melyik cellába érkezett. A cellák költségének kiszámítása ezen adatbázis szerint, minimax algoritmussal történik: Vegyünk egy ellenfelet, aki látván az ágens megcélzott celláját, az eddig tapasztaltak közül bármely hármast kiválaszthatja eredményként. A cellákat úgy pontozzuk, hogy számítunk arra, hogy az ellenfél mindig a számunkra legrosszabb lehet˝oséget fogja választani, így növelve az út hosszát, mi pedig próbáljuk ezt minél kisebbre venni. Nulladik lépésként egy cella reprezentálja az állapotteret és egy másik – az els˝on belül – pedig a cél cellát tartalmazza. Helyezzük az ügynököt a start pozícióba, majd utasítsuk, hogy haladjon a szomszédos cellák közül annak a közepe felé, amelyik a legrosszabb eshet˝oségeket feltételezve a legközelebb van a célhoz. A mozgás tapasztalatát jegyezzük föl az adatbázisba, esetlegesen osztályozzuk újra a cellákat, majd haladjunk a következ˝o cella közepe felé, és így tovább. Ha az ügynök vesztes cellában tartózkodik – vagyis a jelenlegi diszkretizáció mellett nem létezik út a célba – akkor vágjuk mindazon vesztes cellákat ketté a hosszabbik oldaluknál, amelyek szomszédosak egy nyer˝o cellával és minden olyan nyer˝o cellát is, amelyek egy vesztessel szomszédosak, majd folytassuk az ügynök útkeresését (vesztes cella az, amelyb˝ol a legrosszabb eshet˝oségeket feltételezve nem létezik út a célba, nyertes pedig amelyikb˝ol létezik).

16

2.2.1.4.

Egyéb módszerek

Mindkét algoritmus problémája, hogy nagy cellaméret esetén a generált út – bár gyorsan elkészül – de kevésbé lesz optimális, mint kis cellaméret esetén. Megoldást nyújthat a Framed Quadtree[2] használata, amely minden, minimálisnál nagyobb cella köré egy minimális cellákból álló határt képez – melyek között speciális szomszédsági viszony áll fenn – és így hajtja végre a fenti eljárások valamelyikét. Az itt felsorolt algoritmusok alkalmazása f˝oleg kétdimenziós környezetekben megszokott, ugyanakkor az általunk készítend˝o robotnak háromdimenziós térben kell mozognia. Bár mindkét módszernek van olyan változata, amely alkalmazható többdimenziós környezetben, nyilvánvaló, hogy lényegesen kisebb számítási kapacitást igényel a síkbeli változat, ezért próbáltam egy olyan megoldást találni, amellyel alkalmazható a kétdimenziós változat háromdimenziós környezetben olyan eseteket is beleértve, amikor a robotnak például egy spirális rámpát kell használnia a közlekedéshez. Szerencsére a közelít˝o cella dekompozíciós algoritmusokat viszonylag egyszer˝uen át lehet alakítani úgy, hogy a mi esetünkben is használhatóak legyenek. Amennyiben a robot egy olyan, tetsz˝oleges domborzatú térben tud csak közlekedni, ahol nem létezik egymás fölé nyúló, elérhet˝o útvonal, az algoritmusok a következ˝o megszorításokkal használhatóak: – két cellát csak abban az esetben veszünk szomszédosnak, ha a találkozási pontjukon a robot biztonságosan tud közlekedni, nem d˝ol föl, stb. Amennyiben a két cella találkozásánál egyes részek biztonságosak, mások nem, úgy a cellák közül az egyik – lehet˝oség szerint az, amelyikben a probléma oka található – kevert cellának lesz jelölve. – egy cella csak abban az esetben teljesen üres, ha a teljes területén, bármely irányban biztonságosan lehet közlekedni, ha csak részben, akkor kevert. E megkötések alkalmazásához természetesen szükséges az, hogy képesek legyünk tesztelni az adott terület átjárhatóságát, de ez els˝o közelítésben nem t˝unik bonyolult problémának. Amennyiben léteznek egymás fölé nyúló, járható felületek, úgy belátható, hogy a fönti algoritmus minden változtatás nélkül alkalmazható itt is, ugyanis az eljárás nem veszi figyelembe a cellák térbeli elhelyezkedését, csak és kizárólag az egyes cellák közötti átjárhatóságot. Például ha egy „A” cella egy másik, „B” cella „C” szomszédja fölött helyezkedik el, úgy „B” és „C” szomszédosak lesznek, „A”-ból viszont nem lesz él a többibe, így olyan út sem készülhet, amely lehetetlen függ˝oleges irányú mozgást kényszerítene a robotra.

17

3. fejezet Rendszerterv 3.1.

A projekttagok feladatai

Berecz Szabolcs A projektmunkálatok felügyelete, a modellkezel˝o modul létrehozása, tesztelése, valamint a modulok összehangolása. Kovács Róbert A navigációs modul létrehozása, tesztelése. Kúthy El˝od Zoltán A képfeldolgozó modul létrehozása, tesztelése.

3.2.

Képfeldolgozó modul (Kúthy El˝od Zoltán)

A képfeldolgozó modul feladata a kamerák által szolgáltatott képi információ átalakítása a modellalkotáshoz szükséges adathalmazzá. Els˝o körben az Intel által kiadott OpenCV csomag segítségével valósítjuk meg az élpontkiemelést[18]. A kapott pontokat a pont és környezetének intenzitása alapján párosítjuk össze. Amennyiben az így kapott pontpárok nem adnak megfelel˝o eredményt, akkor a képet transzformáljuk a monogenikus jellé[6] a Felsbergféle algoritmus[11] segítségével, majd ezen jelb˝ol választjuk ki a képpár legjellegzetesebb, azaz a legtöbb információt tartalmazó pontjait. Ekkor a kiválasztás energiamaximum kereséssel fog történni. A kiválasztott pontokat úgy párosítjuk össze, hogy a jellemz˝okben lehet˝oleg kis eltérések legyenek. Ezen kritérium alól kivételt képez az irány jellemz˝o, mivel az irányok eltérése függhet a távolságtól. Kérdéses a fenti adatok alapján a felületeket leíró háromszögek meghatározásának módja, mely a modellkezel˝o modul számára alapvet˝o információ, lásd b˝ovebben a modul leírásánál. Felmerült, hogy a háromszögeket valamely éldetektáló algoritmus alapján meghatározott egyenesek segítségével alkotjuk meg, 18

esetleg Hough transzformációt is alkalmazva. Alternatív megoldást jelenthet, ha a háromszögeket közvetlenül a monogenikus jel jellegzetes pontjaiból határoznánk meg, kérdéses a módszer pontossága. Fontos kérdés a kamerák egymáshoz viszonyított helyzete, és a kamerák távolsága. Párhuzamos kamera-elhelyezkedés és nagy kameratávolság alkalmazása nagy látótávolságot és pontos helymeghatározást eredményez, viszont jelent˝os holttérrel kell számolnunk (3.1. ábra). A kameratávolság illetve az optikai tengelyek metszéspontjának kameráktól vett távolságának csökkentésével a holttérrel együtt a látótávolság is csökken (3.2. ábra). A kamerák számunkra optimális helyzetének meghatározása a kés˝obbiek során, gyakorlati tapasztalatok alapján történik.

3.1. ábra. Párhuzamos optikai tengelyek

3.3.

3.2. ábra. Összetartó optikai tengelyek

Modellkezel˝o modul (Berecz Szabolcs)

A modul több – háromszögekkel ábrázolt – felületmodell kezelését végzi, és két f˝o részre bontható: aktuális pozíció meghatározása, valamint a modell építése.

3.3.1. Pozíció meghatározás A képfeldolgozó modultól kapott – lokális koordinátarendszerben lév˝o – pontok, valamint a navigációs részt˝ol kapott – várható elmozdulásra vonatkozó – információk alapján az ICP[4] módszer alkalmazásával megpróbálja meghatározni 19

az aktuális pozíciót. Ha a pozíció meghatározása sikertelen (pl. egy nagymérték˝u, váratlan elmozdulás, vagy bekapcsolás utáni ismeretlen aktuális pozíció miatt), akkor a spin-image módszer[10] alkalmazásával közelít˝o pontossággal kiszámítja az aktuális pozíciót, majd az ICP módszerrel tovább pontosítja azt.

3.3.2. Modellépítés Az aktuális pozíció meghatározása után a modellt kiegészíti az új pontokkal, valamint a modellben már szerepl˝o pontokat tovább pontosítja. A koordináták pontosítása viszonylag egyszer˝u feladat, csak az összetartozó pontokat kell meghatároznunk, majd a két pont hibája és pozíciója alapján módosítjuk a modellben lév˝o pontot. Nem ennyire triviális a modell kiegészítése, mivel számítanunk kell arra, hogy a különböz˝o kamerapozíciókból kismértékben eltér˝o szerkezet˝u felületet kapunk (más jellegzetességeket emel ki a képfeldolgozó modul). Mivel kiegészítés közben sok speciális esettel kellene foglalkozni (pl. egymást fed˝o, egy síkban lév˝o és különböz˝o méret˝u háromszögek), melyek a modell térképként való használatát nem befolyásolják, az implementálás els˝o fázisában a javítással nem foglalkozom. Az egyetlen (nem elhanyagolható) hatása a javítás mell˝ozésének az, hogy a modellben lév˝o pontok és felületek száma gyorsabban n˝o, mint szükséges lenne, ezt legegyszer˝ubben (és valószín˝uleg leghatásosabban) úgy orvosolhatjuk, ha id˝onként „letisztítjuk” a modellt. A „tisztítás” egyetlen nagy hátránya, hogy id˝oigényes, így nem kerülhet˝o el a kiegészítés közben történ˝o ellen˝orzés és javítás. 3.3.2.1.

Hibás adatok kezelése

A modell építése közben számítani kell hibás adatokra is, melyek valójában nem létez˝o felületrészletek érzékelésében nyilvánul meg. Ezen hibák kisz˝urésének egyik lehetséges módja egyfajta meger˝osítéses tanulás alkalmazása. A tanulást egy-egy felületrészletre csak addig alkalmazzuk, amíg meg nem bizonyosodtunk a felületrészlet valódiságáról, melynek feltétele, hogy adott id˝o alatt különböz˝o pozíciókból is érzékeljük a kérdéses felületrészletet. A különböz˝o pozíciókból való érzékelésre azért van szükség, mert ha a robot egy id˝ore megáll, akkor a kameraképek változatlansága miatt minden felületet valóban létez˝onek fogadnánk el. Azzal, hogy csak korlátozott ideig vizsgáljuk egy felületrészlet tényleges létezését, kizárjuk ugyan a környezet változásának folyamatos érzékelését, viszont lehet˝ové tesszük a hatékony m˝uködést, mivel csak néhány felületrészlet esetében kell figyelnünk az „elévülésre”.

20

3.3.2.2.

Mozgás érzékelése

A környezet változásának érzékelésére használhatjuk a hibás felületrészletek kisz˝urésének módszerét azzal a módosítással, hogy csak azoknak a felületrészleteknek a valódiságát vizsgáljuk, amelyek a kamerák látószögén belül, az éppen látható felület és a kamerák között helyezkedik el. Ezen felületek meghatározásának gyorsítására is felhasználható a már említett kd-tree adatstruktúra (2.1.2.4. pont). Ez a módszer nem követi elég gyorsan a változásokat a folyamatos mozgás érzékeléséhez, viszont lehet˝ové teszi, hogy a rendszer észrevegye egy akadály elt˝unését.

3.4.

Irányítás modul (Kovács Róbert)

3.4.1. A robot irányítása A robot szoftverének e modulja felel˝os a robot irányításának minden mozzanatáért. A modul a következ˝o részekre bontható: – A generált modell hiányosságainak, vagyis a felderítend˝o területeknek a meghatározása – A következ˝o elérend˝o pozíció kiválasztása – Út generálása a választott pozícióhoz – A hordozójárm˝u vezérlése 3.4.1.1.

A generált modell hiányosságainak meghatározása

A Modellkezel˝o modultól kapott, háromdimenziós modellt a rendszer els˝o lépésben egy felülnézeti, kétdimenziós térképpé alakítja, majd meghatározza azokat a területeket, amelyeket belát és azokat, amelyek takarásban, vagy túl távol vannak (3.3. ábra). Következ˝o lépésként ezt cellákra bontja (a felderített-felderítetlen részek határainál nagyobb felbontással, lásd quadtree algoritmus[12]), és az újonnan felderített és a felderíthetetlen területeket feltünteti a tárolt térképben – amennyiben nem létezik még ez a térkép, úgy a jelenlegit menti el. Felderíthetetlen terület például egy objektum belseje lehet.

21

3.3. ábra. Föltérképezett terület

3.4.1.2.

A következ˝o elérend˝o pozíció meghatározása

A tárolt térkép „fehér foltjai” közül ez az algoritmus határozza meg azt, amelyiket a legközelebbi mozgás alkalmával el kell érni. Az algoritmusnak figyelembe kell vennie többek között a robot irányultságát, sebességét, helyzetét és esetleges további körülményeket is. 3.4.1.3.

Út generálása a választott pozícióhoz

A robot jelenlegi terveim szerint az út létrehozásához a parti-game algoritmust fogja használni. A cél meghatározása után a parti-game algoritmusban leírt, ágenses módszerrel kerül az oda vezet˝o út megtervezésre (lásd az irodalomkutatás navigációs részét). Amennyiben nem sikerül utat találni a célig, úgy új cél meghatározását kéri a rendszer. Amennyiben a bels˝o heurisztika szerint ez a cél nem csak ideiglenesen elérhetetlen a környezet felderítetlen mivolta miatt, úgy bejelöl˝odik a térképen, mint elérhetetlen cella.

22

3.4.1.4.

A hordozójármu˝ vezérlése

Egyenl˝ore nem áll rendelkezésünkre információ arról, hogy milyen módszerrel lehet majd irányítani a hordozójárm˝uvet, így kénytelen vagyok azt írni, hogy az algoritmus csak a kés˝obbiekben lesz kidolgozva. Annyit mindenesetre feltételezek, hogy valamilyen, a játékautók irányításához hasonló, esetleg sebességszabályozást is lehet˝ové tev˝o kezel˝ofelülete lesz a járm˝unek, amely ugyanakkor nem lesz túl pontos.

3.4.2. A robot mozgásának kalibrálása Amennyiben rendelkezésünkre állna a robot sebességéhez viszonyítva gyors helymeghatározási rendszer, úgy a navigációs rendszer képes lenne a hordozójárm˝uvet oly módon irányítani, hogy miután egy pontatlanul megadott irányba, pontatlan sebességgel elindította a járm˝uvet, a helymeghatározási rendszer segítségével folyamatosan korrigálná a mozgást a szerint, hogy mennyivel tér el a számítottól. Ha viszont e helymeghatározási rendszer nem elég gyors, úgy túl nagy lesz az eltérés az el˝ore tervezett˝ol, esetleg akkora, hogy a robot nekiütközik valaminek, vagy képtelen visszatalálni a tervezett pozícióba. Amennyiben egyáltalán nem állna rendelkezésünkre helymeghatározási rendszer, úgy csak abban az esetben lennénk képesek megfelel˝oen irányítani a járm˝uvet, ha teljesen pontos adatokkal rendelkeznénk az egyes irányítási utasítások hatásairól, vagyis hogy mi lesz a pozíció azok végrehajtása után. Ez pedig csak speciális körülmények között valósítható meg: teljesen sík domborzat, egyenletes talajviszonyok, egyenletesen járó motor, vagyis komoly lehet˝oségként nem jöhet szóba. A problémát az el˝oz˝o két megoldás ötvözésével tervezzük megoldani: egy kezdeti, a vezérl˝o utasítások hatásainak durva beállítása után a robot folyamatosan korrigálná ezen adatokat. E kalibráció a navigációs és a modellkezel˝o rendszer közötti, folyamatos kommunikáción alapul. A navigációs rendszer a tárolt kalibrációs adatok alapján meghatározza ama járm˝uvezérl˝o utasítások sorozatát, amelyekkel belátása szerint a kívánt pozícióba jut a robot, majd a mozgás során folyamatosan lekérdezi az alatta lév˝o, modellkezel˝o rendszert a robot általa ítélt helyzetér˝ol, majd ezen információk alapján korrigál a kalibrációs adatokon – továbbá természetesen a jelenlegi mozgáson is, így a robot képes lesz lejt˝on vagy nehéz terepen (pl.: sz˝onyeg) is közlekedni. Abban az esetben, ha lehet˝oség lesz a járm˝u sebességének szabályozására, úgy a robot képes lesz nagy bizonytalanságot produkáló talajon is közlekedni: ha túl nagy az eltérés a tervezett és a tényleges pozíció között, akkor egyszer˝uen lelassítja a robotot.

23

3.5.

Tesztelés

Els˝o körben a képfeldolgozó és modellkezel˝o modulok tesztelése a navigációs modultól elkülönülve történne. Hiteles bemeneti információt a kamera „rángatásával” állítanánk el˝o. Az ebb˝ol el˝oállított modellb˝ol levont következtetések hozzásegíthetnek számos függ˝oben maradt kérdés megválaszolásához. A navigációs modul tesztelése kezdetben rajzolt térképekkel fog történni. Miután az alrendszerek önmagukban már elfogadható m˝uködést mutatnak, következhet az integrált rendszer tesztje, kezdetben szintén „rángatós” módszerrel. Végs˝o szintet a navigációs modul által vezérelt mobil robot in vitro tesztje képezné.

24

4. fejezet Megvalósítás 4.1.

Képfeldolgozó modul (Kúthy El˝od Zoltán)

A képfeldolgozó modul feladata a sztereó képpárok feldolgozása olyan módon, hogy meghatározza a környezetet reprezentáló pontok koordinátáit a robothoz rögzített koordinátarendszerben. A mélység meghatározásához – hasonlóan az emberi szemhez – szükséges a két kép eltéréseinek kiértékelése, illetve a kamerák küls˝o és bels˝o paramétereinek ismerete. A modul C++ nyelven, került implementálásra, az Intel cég által fejlesztett OpenCV csomag felhasználásával. A képeket két darab Logitech QuickCam Zoom USB csatlakozású webkamera szolgáltatja.

4.1.1. Kamerakezelés El˝oször a két kamerát az OpenCV csomag nyújtotta szolgáltatással kezeltük. Sajnos, ez az eljárás csak egy kamera esetén járható út, mivel a két kamera párhuzamos alkalmazása esetén a képfrissítés 3-4 frame/sec-es értékre esett. Jelenleg a kamerakezelés DirectShow segítségével történik, így már két kamera párhuzamos alkalmazása esetén is elfogadható a képfrissítés sebessége[19].

4.1.2. Kamerakalibráció A képpontok háromdimenziós koordinátáinak rekonstrukciójához a vízszintes diszparitások meghatározásán túl ismernünk kell az effektív pixelméretet a kamerától vett távolság függvényében, illetve a pixeldiszparitások pixelértékeihez rendelhet˝o távolságot. Utóbbi segítségével határozhatjuk meg adott pont pár diszparitásértéke alapján a pont pár mélység koordinátáját, az el˝obbi szükséges a pixel 25

koordinátákról távolság koordinátákra való áttéréshez. Mindkét függvényt egy kalibrációs képsorozattal határoztuk meg, mely egy sakktábla mintázatot tartalmaz, és ismert a kiemelt pontok valós távolsága, illetve a kamera és tábla távolsága. Az 4.1. ábrán láthatóak a kamerakalibráció eredményei, az els˝o pontsorozatra lineáris illesztést végeztünk, a másik két méréssort exponenciális függvénnyel közelítettük.

(a) Effektív pixelméret a távolság függvényében

(b) Képponttávolság a diszparitás függvényében

(c) Effektív pixelméret a diszparitás függvényében

4.1. ábra. A kamerakalibráció eredményeként kapott görbék

26

4.1.3. A megfelel˝o pontok kiválasztása El˝oször ki kell választanunk a képek azon jellemz˝o pontjait(tokeneket), melyeket kés˝obb össze tudunk párosítani a két képen. Ezen feladat megoldására az OpenCV csomag GoodFeaturesToTrack függvényét alkalmaztuk. Az algoritmus a következ˝o lépésekb˝ol áll[18]: – Minden képponthoz meghatározzuk a következ˝o 2 × 2-es kovariancia mátrixot: # " P P (dI/dx)2 (dI/dx)(dI/dy) P , (4.1) K= P (dI/dx)(dI/dy) (dI/dy)2 melynek elemeit az adott pont 3 × 3-as környezetében lév˝o pontok x illetve y irányú deriváltjainak összegeként képezzük.

– Kiszámítjuk a mátrix sajátértékeit, a következ˝o egyenlet alapján: det(K − λE) = 0,

(4.2)

ahol λ a sajátértéket, E pedig az egységmátrixot jelöli, ebb˝ol λ-t kifejezve: P 2 P 2 √ P 2 P 2 2 P 2P 2 P P Dy −( Dx Dy )2 ) Dx + Dy ± ( Dx + Dy ) −4( Dx , (4.3) λ1,2 = 2 ahol Dx az x irányú, Dy az y irányú deriváltat jelöli. Minden képponthoz hozzárendeljük a két sajátértéke közül a kisebbet. – A képet 3 × 3-as ablakokra osztjuk, és a továbbiakban csak azon pontokat tarjuk meg, melyhez tartozó sajátérték lokális maximum az adott ablakra (Non-maxima suppression). – Azon pontokat is eldobjuk, melyekhez tartozó sajátérték nem éri el a legnagyobb sajátérték bizonyos hányadát. Ez a százalékérték a függvény egyik paramétere(t). – Végül a függvény másik paramétereként (dmin ) megadott pixeltávolságnál egymáshoz közelebb lév˝o pontok közül csak a legnagyobb sajátértékkel rendelkez˝ot tartjuk meg. A két képb˝ol a fenti algoritmussal kiválasztott pontokat kell összepárosítani. Az eljárás hatékonyságát természetesen dönt˝oen a két paraméter értékének megfelel˝o megválasztásán múlik. A vizsgált tesztképek esetében t = 5 − 10%, dmin = 5 − 10 paraméter értékeknél sikerül a legjobb eredményeket elérni.

27

(a) t = 2%, dmin = 10

(b) t = 10%, dmin = 5

(c) t = 10%, dmin = 10

(d) t = 10%, dmin = 25

4.2. ábra. Token-detektálás különböz˝o paraméter értékek esetén

4.1.4. Pontpárok képzése A két képen nagy valószín˝uséggel azon pontok egyeznek meg, melyeknek intenzitáskülönbsége illetve környezetük intenzitáskülönbsége minimális. Ezért minden lehetséges párosításhoz meghatározzuk a következ˝o értéket: D=

1 1 X X

i=−1 j=−1

|ax+i,y+j − bu+i,v+j |,

(4.4)

ahol an,m a baloldali kép (n, m) koordinátájú pontjának, bn,m a jobboldali kép (n, m) koordinátájú pontjának intenzitását jelöli, (x, y) a baloldali kép tokenjének koordinátája, (u, v) a jobboldali kép tokenjének koordinátája. Ideális esetben a minimumértékekhez tartozó párosítások a helyesek. Mivel a lehetséges párosítások száma a tokenszámmal exponenciálisan növekszik, illetve nem biztos, hogy 28

adott tokennek van párja – pl. takarásban van csak az egyik képen a pont -, ezért a következ˝o megszorításokat alkalmazzuk a párosítás során: – Mivel sztereó képek esetén a pontpárok y irányú diszparitása nem túl nagy, csak olyan pontpárokat vizsgálunk, melyek y koordinátájának eltérése egy meghatározott értéknél kisebb (320 × 240-es felbontású kameraképekhez 5 pixeles eltérést használtam). – Szintén korlátozzuk a maximális x irányú eltérést is, természetesen itt jóval nagyobb értéket alkalmazunk. Ezzel az algoritmus gyorsítása mellett a párosítás hibáját is csökkentjük, mivel elképzelhet˝o, hogy több hasonló intenzitáskörnyezet˝u token is található a képen, melyek közül a triviális rossz (nagy távolságra lév˝o) párosításokat kisz˝urjük ezzel a korlátozással. A programban ez a korlát 50 pixel, 320 × 240-es felbontású képpárok esetén. – Az algoritmus a baloldali tokenekhez keresi a jobboldali párjukat meghatározva az intenzitásbeli eltérésüket (4.4 egyenlet), de miel˝ott párként regisztrálná o˝ ket, megvizsgálja, hogy az adott jobboldali tokenhez nem létezik-e kisebb eltérés˝u baloldali token. Amennyiben talál ilyet a pontpárt csak lehetséges párként jegyzi fel, ellenkez˝o esetben biztos párként. A lehetséges párok feljegyzésére azért van szükség, mivel az el˝obbiekben megemlített második párkeresés alkalmával talált baloldali tokenhez tartozhat kisebb differenciájú jobboldali token, így jó párosítások is elveszhetnek. Miután az összes baloldali tokenen végigment az algoritmus, megvizsgáljuk, hogy a lehetséges párok közül melyek maradtak párosítatlanok, ezeket szintén biztos párként jegyezzük fel. A vizsgált képek esetén a program a lehetséges párok 80 − 90%-át találta meg, a párok 3 − 5%-a volt hibás.

4.1.5. A mélység meghatározása A pontpárokból az alábbi egyenletekkel határozzuk meg a háromdimenziós koordinátákat, ahol az origó a két lencse középpontját összeköt˝o szakasz felez˝opontjában helyezkedik el: xb + xj − ox )sx 2 yb + yj − oy )sy Y = −( 2 Z = −f (xb − xj ),

X = −(

29

(4.5) (4.6) (4.7)

4.3. ábra. A pontpárok ábrázolása a két kép átlagán

ahol xb , yb , xj , yj a bal- illetve jobboldali token képkoordinátája, ox , oy optikai középpont képkoordinátája, sx , sy pixel x és y irányú mérete, f a kamerakalibráció során a mérési pontokra illesztett exponenciális függvény. Az így el˝oállt háromdimenziós koordinátákat tartalmazó tömböt adjuk át a modellkezel˝o modulnak.

4.4. ábra. Megtalált pontpárok összekötve

4.2.

Modellkezel˝o modul (Berecz Szabolcs)

A modul egyszer˝u részeit C++ nyelven, a bonyolultabb részeket Python nyelven implementálom, mivel ez utóbbi nyelv jobban megfelel az algoritmus prototípusok gyors elkészítésére és tesztelésére. A kés˝obbiekben, mikor már fontos lesz a kis számításigény, a problémásabb programrészek viszonylag egyszer˝uen átültethet˝oek C++ nyelvre. 30

Ahhoz, hogy a két nyelv együttm˝uködhessen, szükség van egy interfészre, melynek létrehozása elméletileg kézzel is lehetséges, gyakorlatilag viszont csak stabil C++ objektum-interfészek esetén kivitelezhet˝o (fáradságos – és felesleges – munka árán). Ehelyett használhatunk olyan programokat/library-ket, melyek bizonyos szintig automatizálják ezt a feladatot. Az általam használt library, a Boost.Python[5] egy viszonylag egyszer˝u interfész definíció alapján hozza létre a két nyelv közötti átjárást biztosító interfészt. Az interfész definíció egy egyszer˝u C++ file, ami a Pythonból használni kívánt osztályok, függvények, stb. neveit, és néhány kiegészít˝o információt tartalmaz (pl. abban az esetben, ha egy függvény visszatérési értékének típusa pointer vagy referencia, meg kell adnunk, hogy a Python milyen módon kezelje ezt az objektumot, mert a deklarációk alapján nem dönthet˝o el hogy a hivatkozott objektum felszabadítása kinek a feladata). Ezt a C++ file-t lefordítva kapunk egy object file-t, amit már használni tud a Python interpreter. Az interfész definíció létrehozását is kikerülhetjük, ha használjuk a Pyste nev˝u programot, mely egy C++ header file alapján létrehozza helyettünk az interfészt definiáló C++ file-t, de így sem úszhatjuk meg a pointerek, referenciák kezelési módjának megadását. Ezek után a C++-ban implementált algoritmusok és típusok egyszer˝uen elérhet˝ok Pythonból is, s˝ot teljesen úgy viselkednek, mintha Pythonban implementáltuk volna: m˝uködik az öröklés, a C++-ban megírt osztályokhoz hozzáadhatunk újabb metódusokat, vagy egy létez˝ot lecserélhetünk, stb.

4.2.1. Modellek ábrázolása A modellek ábrázolása háromszögekb˝ol álló felületekkel történik. Egy teljes felület leírásához négy osztály szükséges: v3d, vertex, face és mesh osztályok. A négy osztályból három szorosan összekapcsolódik, gyakorlatilag egyik sem használható a másik kett˝ot˝ol függetlenül. mesh Ez egy a vertex és face osztályok példányait aggregáló osztály. A példányokat egy-egy tömbben tárolja, így egyszer˝uen hivatkozhatunk az egyes vertex-ekre és face-ekre, valamint lehet˝ové teszi néhány m˝uvelet egyszer˝u implementálását. v3d Egy egyszer˝u háromdimenziós vektort valósít meg. A v3d osztályon értelmezett m˝uveletek a szokásos vektorm˝uveletek: összeg, különbség, skaláris és vektoriális szorzat, skalárral való szorzás, hossz, normalizálás, és két vektor különbsége. vertex A felület egy pontját írja le. Ennek két bázisosztálya van: a v3d és a markable osztály. Számon tartja azoknak a face-eknek az indexét, melyek egyik csúcsa ez a pont. 31

face A felület egy részletét ábrázolja három vertex segítségével. A vertex-ekre azok mesh-beli indexével hivatkozik. Az osztálynak egyetlen bázisosztálya van, a markable osztály. További osztályok egyszer˝usítik a felületek kezelését, ezek, valamint az el˝oz˝oekben röviden bemutatott osztályok b˝ovebb bemutatása következik. 4.2.1.1.

A mesh osztály

A pozíció meghatározása, valamint a már meglév˝o modell kiegészítése szükségessé teszi a Kd-tree struktúra használatát, melynek Martin F. Krafft által implementált változatát használom[13]. 4.2.1.2.

A vertex és face osztályok

Ez a két osztály lehet˝ové teszi, hogy kiszámítsuk a felület egy vertex-éhez, vagy face-éhez tartozó normálvektort. Mivel egy face-hez tartozó normálvektor meghatározásához ismernünk kell a face-t alkotó vertex-ek pozícióit, egy vertex normálvektora pedig a környez˝o face-ekb˝ol számítható, könnyen elérhet˝ové kell tenni az egyik osztályból a másikat. Ennek legegyszer˝ubb módja, ha minden vertex és face osztály számon tartja – egy pointer segítségével –, hogy melyik mesh tartalmazza o˝ ket, majd ezen a mesh-en keresztül kérdezi le a szükséges információkat. Ez egy nagyméret˝u mesh esetén nagy memóriaveszteséggel jár, viszont lehet˝ové tesz néhány konzisztencia-ellen˝orzést, valamint szükség esetén viszonylag kis módosításokkal eltávolítható ez a hivatkozás. A számításigény is csökkenthet˝o, ha eltároljuk a már kiszámított normálvektorokat, viszont ez tovább bonyolítja egy mesh módosítását, így csak akkor érdemes megvalósítani, ha a kód nagy része már viszonylag keveset változik. 4.2.1.3.

A breadth_first_vertex_iterator osztály

Néhány esetben (pl. a 2.1.2.3. pontban bemutatott spin-image módszerre specializált ICP algoritmusban) szükséges a felület egy adott vertex-ét˝ol egyre távolodva bejárni a felület vertex-eit. Ezt egy szélességi kereséssel lehet megoldani, ahol az aktuális vertex-hez tartozó face-ek vertex-eit egy FIFO-ba helyezzük, a következ˝o vertex-et pedig a FIFO-ból egy elemet kivéve kapjuk. Ezzel a módszerrel egy vertex többször is bekerülhet a FIFO-ba, így fel kell jegyeznünk, hogy melyik vertex-et használtuk már fel. Ezt a feladatot a marker és a markable osztályok oldják meg (4.2.1.4. pont).

32

Az osztály két fontos metódussal rendelkezik: next() Ez a Python nyelv iterátor interfészéhez tartozik, a feladata meghatározni a következ˝o elemet, vagy ha nincs több, egy StopIteration kivételt dobni. turn_back() Ha az aktuális pont szomszédait nem szeretnénk bejárni, ennek a metódusnak a segítségével jelezhetjük ezt. A turn_back() metódus megvalósításához az objektum külön tárolja az utolsó next() híváskor meghatározott szomszédokat, így ha ebben az irányban nem akarunk továbbmenni, a turn_back() metódus egyszer˝uen eldobja ezeket az elemeket, egyébként viszont a következ˝o next() hívás els˝o lépésként ezen elemeket a FIFOba teszi. A következ˝o Python kódrészletb˝ol látszik, mennyivel átláthatóbbá teszi ez az osztály a vertex-ek bejárását. A kódrészlet a model_mesh 0 index˝u vertex-ét˝ol legfeljebb öt egység távolságra lév˝o szomszédos vertex-eket gy˝ujti össze a nearby_vertex_neighbours listában: vertex_iter = breadth_first_vertex_iterator(model_mesh, 0) base_vertex = model_mesh.get_vertex(0) vertex_neighbours = [] for v in vertex_iter: if base_vertex.distance(v) > 5: vertex_iter.turn_back() else: vertex_neighbours.append(v) Ezzel a módszerrel egyszer˝uen létrehozható egy depth_first_vertex_iterator osztály is, mely mélységi kereséssel járja be a felület pontjait. Az implementációban az egyetlen különbség a bejárandó vertex-ek tárolásának módja: FIFO helyett LIFO-t kell használnunk. 4.2.1.4.

A marker és markable osztályok

A két osztály segítségével objektumokat jelölhetünk meg valamilyen céllal (4.2.1.3. pont). A megjelölni kívánt osztály egyik o˝ sosztályának a markable osztályt választjuk, amely az utolsó jelzés azonosítóját tárolja. A marker osztály minden objektuma különböz˝o azonosítóval rendelkezik. A mark() metódus segítségével megjelölhetünk egy markable objektumot (beállítjuk a markable objektum által tárolt azonosítót az aktuális marker objektum azonosítójára), valamint az is_marked() metódussal összehasonlíthatjuk az azonosítókat. 33

4.2.1.5.

A indexed_v3d osztály

Mivel a vertex-ek tömbben történ˝o tárolása sok feladat megoldását egyszer˝usíti, a Kd-tree adatstruktúra implementációja pedig saját adatterületen tárolja a pontokat, szükség van egy leképezésre a Kd-tree-ben tárolt pontok és a mesh osztályban tárolt vertex-ek között. Ennek megvalósítása az indexed_v3d osztállyal (ennek bázisosztálya a v3d osztály) történik, mely a pont koordinátáin kívül annak mesh objektumban elfoglalt helyét is tárolja. Ez gyakorlatilag kétszeres memóriaigényt jelent, de lehet˝ové teszi a modellek gyors kezelését.

4.2.2. A spin_stack osztály Ez az osztály tárolja egy mesh objektum által leírt felület pontjaihoz tartozó spin-image-eket. Mivel a mesh osztály egy tömbben tárolja a felület pontjait, kézenfekv˝o a választás, hogy a spin_stack osztály is egy tömböt használjon az egyes pontokhoz tartozó spin-image-ek tárolására, így ugyanazzal az indexszel hivatkozhatunk egy pontra, és az ahhoz tartozó spin-image-re. Az implementáció többi részlete gyakorlatilag teljesen megegyezik a Johnson[10] által leírttal. 4.2.2.1.

Teszteredmények

Mivel valódi adatok még nem állnak rendelkezésre, így a tesztelést a Johnson által használt kacsa modellen (2.1. ábra), egy kézzel összeállított „szoba” modelljén (4.5. ábra), és ezek zajos változatán végeztem (mindegyik modell kb. ezer pontból áll). A hozzáadott zaj mértékét a modell tömegközéppontjától számított átlagos ponttávolsághoz viszonyítva határoztam meg. Az algoritmus paramétereinek finomhangolásához több – valódi – adatra lenne szükség, így az algoritmus hatékonyságát ezen eredmények alapján nem lehet megítélni.

4.5. ábra. A „szoba” modellje

34

A tesztelés során a modelleken nem szükséges transzformációt végezni, mivel a spin-image algoritmus a ponthalmaz bármilyen helyzete esetén ugyanazt az eredményt adja, így az algoritmus hatékonyságának teszteléséhez elég a pontokhoz zajt adni (zaj nélküli, kevés ismétl˝odést tartalmazó modellek esetén mindig helyes eredményt ad). A tesztek egyszer˝uen kiértékelhet˝ok, mivel a zajt tartalmazó modellt az eredeti modell duplikálásával majd egyenletes eloszlású zaj hozzáadásával hoztam létre, így a két modellben a pontok azonos sorrendben szerepelnek, tehát annak ellen˝orzéséhez, hogy helyesek-e a meghatározott pontpárok, elég páronként összehasonlítani a pontok indexét. Egyezés esetén helyes az eredmény. A feladat száz véletlenszer˝uen kiválasztott pont párjának meghatározása volt különböz˝o mérték˝u zajok esetén. Röviden összegezve az eredményeket: a kacsa modellen végzett tesztek eredménye biztató, a „szoba” modell eredménye elmarad ugyan a kacsa modellét˝ol, de szintén biztató. A 4.6. ábrán az egy és két százaléknyi maximális zajt tartalmazó kacsa modelleken végzett ötven teszt eredménye látható. A grafikonokról leolvasható, hogy egy százalék zaj mellett tökéletes az eredmény. Két százalék zaj mellett, megfigyelhet˝o, hogy kell˝oen sok kiindulási pontot felhasználva, a csoportosított pontpárok alapján számított legjobb transzformáció valószín˝uleg helyes eredményt ad. p o n t p á r o k

Kacsa modell, 1% hiba

p o n t p á r o k

50 40 30 20

10 s z 0 á m a -10

10

20 30 teszt sorszáma

40

40 30 20

10 s z 0 á m a -10

△△ △ △△ △ △△ △△ △ △ △ △ △△ △ △△ △ △ △ △ △ △ △ △△ △△ △ △△△ △ △ △ △△△ △ △ △ △ △ 22222222222△ 222△ 2 222222222 222222222△ 2222222△ 22△ 22222222 0

Kacsa modell, 2% hiba 50

50

△ △ △ △ △△ △ △△△ △ △ 2 △ △ △ △ △ △ △ △ △△ △ 22 △ 22△ △ △ △△ △△ △ △ △ △ △ △ △ △ △ 2 2△ 22 2 △ △ 22 222 △ 222△ 22222△ 22 2△ 222 222△ 22△ 2 2 222222 22 2222 22 △ 0

10

20 30 teszt sorszáma

40

50

4.6. ábra. Kacsa modell, egy és két százalék zaj1 A „szoba” modell eredményein (4.7. ábra) látható, hogy a hibázási arány hasonló, mint a kacsa modell esetében, viszont láthatóan kisebb a meghatározott pontpárok száma. Ennek oka az, hogy a „szoba” modell túlzottan mesterséges, így kevés jellegzetes felületrészlet található a modellben: az algoritmusban lév˝o egyik sz˝urés azokat a pontpárokat dobja el, amelyek spin-image-einek hasonlósági mértéke nem kell˝oen eltér˝o a többit˝ol. Ha ezt a küszöbértéket csökkentjük, akkor kevesebb pontot dob el a rendszer, aminek két fontos következménye van: 1

Kék háromszög: helyes, piros négyzet: hibás párosítás

35

p o n t p á r o k

Szoba modell, 1% hiba

p o n t p á r o k

50 40 30 20 10

s z 0 á m -10 a

10

20 30 teszt sorszáma

40

40 30 20

10 s z 0 á m -10 a

△ △ △ △ △△ △ △ △△ △ △ △ △ △ △ △ △ 22 2△ △ △ △ △ △ △ △ △ △ △ △ 2 2△ 2 222 2△ 2 2△ 2 2△ 2 22△ 22△ 2△ 2 22 22 2 22△ 22△ 2 2 2△ 2 2△ 2 2△ 2 2 2△ 2 22 2 2△ 2 22 2△ 2 △ △ △ △ △ 0

Szoba modell, 2% hiba 50

50

△△△△ 2 △ △ △ △ △ △ △ △ △ 2△ 2 2△ 2 2 △ △ △ △ △ △ △ 2 2△ 2 2 2△ 2 2△ 2 2 2 2△ 2△ 2 2△ 2 2△ 2 2△ 2 2△ 2 2△ 2 2△ 2 2 2△ 2 2△ 2 2△ 2 2 2△ 2 22 22△ 22△ 22△ 2 △ △ △ △ △ △ △ △ △ △ 0

10

20 30 teszt sorszáma

40

50

4.7. ábra. „Szoba” modell, egy és két százalék zaj1 – Megn˝o a számításigény, mivel több pontpárból több csoportot hozhatunk létre, és ezek mindegyikére meg kell határoznunk a transzformációt. – Több hibás pontpár jut át a sz˝urésen, ezáltal rontva az utolsó lépésként meghatározott transzformáció helyességét. Tehát a küszöbértéket érdemes magasan tartani, viszont azokban az esetekben, amikor többszöri próbálkozásra sem sikerül kell˝o számú pontpárt meghatározni, ideiglenesen lecsökkenthetjük, hogy kell˝o számú pontpárt kapjunk (ami egyáltalán nem biztosítja, hogy megközelít˝oleg helyes transzformációt sikerül meghatározni, de legalább lehet˝oséget ad rá).

4.2.3. A transzformációk ábrázolása Mivel eltoláson és forgatáson kívül semmilyen transzformációra nincs szükség, felesleges az elterjedt 4×4-es mátrix használata. Hatékonyabban megoldható a pontok transzformációja, és a transzformációk konkatenációja, ha a forgatás leírására egy kvaterniót, eltoláshoz pedig egy vektort használunk. Ezzel az ábrázolásmóddal egy pont transzformációját a következ˝o kifejezéssel végezhetjük el: ˙ q˙ −1 p′ = t + qp Forgatás leírásához egységkvaternióra van szükség, ennek inverzét pedig egyszer˝uen számíthatjuk: negáljuk a kvaternió vektor komponensét. A kifejezés további egyszer˝usítésével (mely egyszer˝uen elvégezhet˝o egy szimbolikus matematikai programcsomaggal, ilyen pl. az általam használt Maxima[14]) azt kapjuk, hogy n pont transzformációjához 18 + 9n szorzásra valamint 12 + 9n összeadásra van szükség.

36

Két transzformáció konkatenációjának kiszámítása: q˙ ′ = q˙ 2 q˙ 1 t′ = t2 + q˙ 2 t1 q˙ −1 2 Egyszer˝usítések után 18 + 25 szorzás és 12 + 12 összeadás marad, melyekb˝ol 18 szorzást és 12 összeadást csak egyszer kell elvégeznünk egy transzformáció vagy egy transzformáció-konkatenáció során. 4×4-es mátrix használata esetén pont transzformációjakor 16n szorzás és 12n összeadás, transzformáció konkatenáció esetén pedig 64 szorzás és 48 összeadás szükséges. Egy másik lehet˝oség, hogy a kvaternió helyett egy 3 × 3-as mátrixot használunk, de mivel az abszolút orientáció meghatározása során kvaternió formájában kapjuk a forgatást, a számításigény nem változik, az átalakítás viszont felesleges pontatlanságokat okoz.

4.3.

Irányítás modul (Kovács Róbert)

Az irodalomkutatás részben említett parti-game algoritmus került kisebb módosításokkal megvalósításra, de folyik egy quadtree alapú D* search implementálása, amely nem kizárt, hogy jobb eredményeket fog produkálni, mint a jelenleg használt. Az út generálása jelenleg a következ˝o lépésekb˝ol áll: – a modellkezel˝o modultól kapott háromdimenziós modell kétdimenziós térképpé alakítása – a parti-game-hez hasonló algoritmus alkalmazásával generálunk egy utat, mely azonban csak egy szabadon szálló, kör alakú, minden irányban végtelen gyorsulásra képes objektum számára járható – a generált utat ezek után megpróbáljuk úgy átalakítani, hogy a valóságos járm˝u is képes legyen azon végighaladni.

4.3.1. A modell térképpé konvertálása A modellkezel˝o modultól kapott térbeli felületeket – lényegében háromszögeket – els˝o lépésben két részre osztjuk meredekség szerint: a túl nagy d˝olést mutatóak egyértelm˝uen tereptárgy címkét kapnak, a vízszinteshez közeliek közül válogatjuk ki azokat, amelyek esetlegesen a robot számára megközelíthet˝oek. 37

Következ˝o lépésként meghatározásra kerülnek az(ok) a felület(ek), amelyeken a robot jelenleg tartózkodik – ezt a kapott pozícióinformáció alapján tesszük meg úgy, hogy a robot méreteit ismervén vesszük azokat a felületeket, amelyek a kerekek alatt helyezkednek el – némi t˝uréssel. Amennyiben a kiindulási hely több felületb˝ol áll, úgy meghatározzuk azokat az éleket és élrészeket, amelyek a "széleket" alkotják, vagyis a háromszögek bizonyos t˝uréssel való egyesítése során az egyesített felület széleit képeznék. Ezek után tovább válogatjuk a vízszintes címkét kapott felületeket olyanokat keresve, amelyek csatolhatóak az eddigiekhez; ha találunk ilyet, akkor azt hozzávesszük az eddig találtakhoz és újra meghatározzuk a virtuális idom széleit, majd addig folytatjuk a keresést, amíg már egyetlen háromszög sem csatolható. A háromszögek szomszédsági viszonyait egy gráfban tároljuk. Az így kiválasztott felületeket följegyezzük a térképre, amennyiben léteznek egymás fölött elhelyezked˝o felületek, úgy több térképre. A térkép az egyes pontjaiban három értéket vehet fel: ISMERETLEN, ÜRES, OBJEKTUM. Inicializálásként létrehozunk egy olyan térképet, amely minden pontjában ISMERETLEN értéket tartalmaz, majd erre rávetítjük a kiindulási helyt˝ol indulva a felületeket egyszer˝uen úgy, hogy a magasság koordinátáikat elhagyjuk. Amennyiben egy felület a térkép olyan részére vetülne, amely már nem ISMERETLEN címkéj˝u, úgy létrejön egy újabb térkép – újabb szint – vagy ha már létezik, akkor ez lesz az aktuális, és erre vetít˝odik rá a felület. Minden felület kap egy tulajdonságot, amely azt mutatja meg, hogy melyik térképre lett ráhelyezve. Ezen tulajdonság és a szomszédsági gráf alapján meghatározzuk azokat az éleket, amelyeket átlépve képzeletbeli barangolónknak egy másik térképen kell folytatnia útját, majd ezeket is bejelöljük a térképen. A feldolgozatlan felületeket megvizsgáljuk, hogy bármely részük olyan tartományban van-e, amely zavarná a robot mozgását. Ha ilyet találunk, akkor ezeket bejelöljük a térképeken, tekintet nélkül arra, hogy a kérdéses térképterületen milyen érték˝u pontok találhatóak. Az összes háromszög feldolgozása után a térképeken lév˝o ISMERETLEN címkéj˝u területek közül kiválasztjuk a felderítésre váró részt, majd ezeket a részeket is objektumként jelöljük.

4.3.2. Az út generálása – A parti-game algoritmus Az implementált algoritmus a parti-game elveire épül, de számos helyen különbözik attól. F˝o különbség, hogy az eljárás az els˝o bejárás során végleges, a környezet aktuális felbontásához viszonyítva optimális útvonalat generál, szemben az eredetivel, ahol az ügynök els˝o próbálkozása általában jóval hosszabb útvonalat produkál mint a második vagy a harmadik esetben. További lényeges eltérés még, 38

hogy az ügynök két típusú tapasztalatot szerezhet szemben az eredeti eggyel: az egyik akkor jön létre, mikor egy cellát megpróbál elhagyni, a másik pedig akkor, amikor egy cella közepét szeretné elérni – ezeket az állapottér további felosztásakor vesszük figyelembe. Az eredeti, ügynökös szemléltetési módszert is csak az egyszer˝uség kedvéért használjuk, mivel jelen esetben közbens˝o célként sem jelenik meg, hogy az ügynököt eljuttassuk a megadott koordinátákra – számunkra a közbens˝o cél az induló cellától a célcelláig vezet˝o út létrehozása. 4.3.2.1.

Inicializáció

Bemen˝o adatként az eljárás kap egy vagy több bináris térképet a környezetér˝ol, továbbá egy start és egy cél koordinátát. Az algoritmusnak lényegében mindegy, hogy egy vagy több térképpel kell dolgoznia, mivel a környezet bels˝o reprezentációja egy szomszédsági gráf, melyben több térkép akár bonyolult összeköttetései is egyszer˝uen ábrázolhatóak. Nem tartozik szorosan az algoritmus keretei közé, de nulladik lépésként egyfajta dilatációt végzünk el, vagyis megnöveljük a tereptárgyak kiterjedését. Ezt a következ˝ok miatt tesszük: a procedúrának nem feladata vizsgálni, hogy az általa megtalált úton egy fizikai kiterjedésekkel rendelkez˝o járm˝u elfér-e, így akkor is talál megoldást, ha az tartalmaz egy vagy több nagyon sz˝uk járatot is. A tereptárgyak "felhízlalásával" viszont elérjük, hogy ezek a sikánok elt˝unjenek, továbbá, hogy az eredeti térképen a generált út és a tereptárgyak között minimum akkora távolság legyen, mint amekkora a szélesítés mértéke volt. Említést érdemel még, hogy definiálunk egy maximális és egy minimális cellaméretet is, ahol a maximális cellaméret csak a környezet els˝o felbontásakor lényeges, a minimális pedig azért szükséges, mert – szemben az eredeti elmélettel – itt nem feltételezhetjük, hogy biztosan létezik út a start és a cél koordináták között. Ezek után a meghatározott maximális méret˝u cellákra bontjuk a környezetet és triviális módszerrel meghatározzuk a szomszédsági viszonyokat is. Az eredeti eljárásban induláskor csak két cella volt: egy, amely tartalmazta az egész állapotteret, és egy másik, amely csak a cél koordinátát. Két ok is volt, amiért ezt a módszert elvetettem: egyrészt kis tereptárgy-s˝ur˝uség esetén – bár gyorsan – de pazarló útvonal generálódik, több akadály esetén pedig id˝obe telik, amíg az állapottér felbontása eléri a megfelel˝o mértéket. Legnagyobb cellaméretnek egyenl˝ore el˝ore definiált értéket használunk, kés˝obbiek során valószín˝uleg a kapott térképen lév˝o objektumok számától függ˝oen fogjuk ezt meghatározni – egyébként az objektumok számának meghatározásával együtt az algoritmus lassabb, mint az eredeti, de mivel ezt mindenképpen meg kell tennünk egy ett˝ol független procedúra használatához, így nem vesszük figyelembe az id˝oigényesség szempontjából. Az egy lépéses útgenerálás és pontosabb érkezési pozíció elérése miatt a start és célkoordinátákat tartalmazó cellákat minimális méret˝ure csökkentjük, a követ39

kez˝o egyszer˝u módszerrel: LOOP: 1. Megkeressük azt a cellát, amelyik a koordinátát tartalmazza 2. Ha ez minimális méret˝u, kilépünk 3. Ha nagyobb, mint a minimális cellaméret, felezzük a cellát A célcella minimális méret˝uvé alakítására azért van szükség, hogy az érkezési pozíciót pontosabban tudjuk elérni, a kiindulási cella átalakítása pedig az ügynök szerepének csökkenése miatt lényeges – az eredeti eljárásban az ügynök pontosan a start pozícióból indult, így az utat is pontosan innen generáltuk. A célcella céltól való távolságát (Goal Distance, továbbiakban GD) 0-nak vesszük, majd az általános esetben is használt algoritmussal meghatározunk egy GD értéket minden cellához úgy, hogy egy cella céltól való távolsága egyenl˝o az egyes szomszédai plusz a szomszédaitól való távolságának minimumával. Két cella távolságát sakktábla távolságot használva határozzuk meg, vagyis egy (x1 , y1 ) középpontú cella egy másik, (x2 , y2 ) középpontú cellától t távolságra van, ha max(|x1 − x2 |, |y1 − y2 |) = t; két cella szomszédos, ha van közös határolószakaszuk. Készülhetett volna speciális algoritmus is a kezdeti céltól való távolságok meghatározására, de teljesen elhanyagolható az az id˝o a teljes futásid˝ohöz képest, amíg az általános algoritmust végrehajtjuk. 4.3.2.2.

A f˝o eljárás

Az algoritmus egy iterációs lépésben megpróbál az aktuális cellából abba a szomszédos cellába jutni, amelyiket a céltól való távolság meghatározásakor választottunk ki. Ezt egészen addig folytatja, amíg vagy el nem éri a cél cellát, vagy az aktuális cella GD értéke végtelen nem lesz – mely esetben a kiválasztott cellákat kisebb méret˝ure vágja és újból megpróbálkozik a célba jutással. Indulásként vesszük azt a cellát, amelyik a start koordinátát tartalmazza, majd megpróbálunk áthaladni a következ˝obe. Ezt oly módon tesszük, hogy az ügynököt az aktuális cella közepébe helyezzük, és utasítjuk, hogy haladjon a két cella közös határolószakaszának közepe felé. Amennyiben akadályba ütközik, úgy az aktuális cellát fölvesszük a szétvágandók közé, a két cella távolságát végtelenre állítjuk és frissítjük a cellák céltól való távolságát is. Ha viszont sikeresen elért a kijelölt pontig, úgy utasítjuk, hogy haladjon a második cella középpontja felé. Ha ez nem sikerül, akkor végrehajtjuk a fent leírtakat azzal a különbséggel, hogy most a második cella kerül be a szétvágandók listájába. Ha az ügynök bármely lépésben akadályba ütközött, úgy az aktuális cella nem változik, ellenkez˝o esetben 40

viszont azt a cellát vesszük aktuálisnak, amelyikben az ügynök éppen tartózkodik, és ezt a cellát föl is jegyezzük egy útvonallistába – melynek a generálása a célunk. Ha a céltól való távolságok frissítése során az útvonallistában szerepl˝o bármely egymás utáni két cella GD értéke növekszik, úgy addig töröljük a lista elemeit, amíg egy GD érték szerint szigorúan csökken˝o sorozatot nem kapunk, majd a legutolsó cellát aktuálissá téve folytatjuk az algoritmust – ez biztosítja, hogy már az els˝o esetben megkapjuk az optimális utat a cél felé. Az eredeti algoritmusban az ügynök egyenesen a következ˝o cella közepébe halad, és ha közben akadályba ütközik, akkor jó eséllyel mindkét cella fölbontását elvégzi. Ez azért rossz, mert hozzávet˝olegesen megduplázza a karbantartandó cellák listáját, melyek értékeinek frissítése veszi igénybe a legtöbb id˝ot. Ansari és Williams javasolták[1], hogy csak a vesztes oldali (vesztes: az, amelyikb˝ol nem vezet út a célba) cellákat bontsuk föl, és változtassuk meg az ügynök célját a két cella határolószakaszának közepére – kiküszöbölend˝o a szomszédos cellák méretbeli különbségéb˝ol adódó problémákat. Az ötlet nem volt alkalmazható az alapelvek közti különbségek miatt, de kiindulási alapot jelentett a jelenlegi verzióhoz. Szintén o˝ k javasolták, hogy a fölbontandó cellák listájából csak a legnagyobb cellákat vágjuk ketté, így biztosítva, hogy elakadás esetén az akadály mentén lév˝o parányi cellák kés˝obb legyenek felbontva mint azok az óriási cellák, amelyeken keresztül létezik út, csak a jelenlegi diszkretizáció mellet azt még nem találtuk meg – ezt az ötletet teljes egészében tudtuk alkalmazni. Ha az iteráció végén a célcellát még nem értük el és van még szétvágható cella, úgy el˝oször kisz˝urjük azokat a cellákat a szétvágandók listájából, amelyek nem a gy˝oztes-vesztes határvonalon fekszenek – így nem vezethetnek be új utat a célig – és a maradék közül a legnagyobbakat felezzük. Minden újonnan létrejött cellával kapcsolatban újra meghatározzuk a szomszédságokat, töröljük az ezekre a területekre vonatkozó tapasztalatokat és újraszámoljuk a céltól való távolságokat, majd újrakezdjük az egész útkeresést a start koordinátától indulva. Annak ellenére, hogy a teljes újrakezdés miatt sok esetben jó útszakaszok is elvesznek, az eljárás olyan gyorsan visszaépíti a már egyszer bejárt utat, hogy nem láttam értelmét erre külön figyelmet fordítani. A teljes algoritmus kétféleképpen érhet véget: vagy az aktuális cella céltávolsága végtelen és nincs több szétvágható cella – és így nem létezik ezzel a minimális cella mérettel út – vagy elértük a célcellát, és így triviálisan el˝oállítható az útvonallistából egy, az aktuális diszkretizáció mellett optimális út. 4.3.2.3.

A cellák céltól való távolságának meghatározása

Mivel az egész algoritmus futási idejének java részét a cellák GD értékének számolásával tölti, fontos, hogy ezt lehet˝oleg minél optimálisabban tegye, így 41

szóba sem jöhetett olyan megoldás, amely minden, a cellák szomszédsági viszonyaiban beállt változáskor az összeset törli és újraszámolja a célcellától kiindulva. Ennél gyorsabb módszerre volt szükségünk, így a következ˝o procedúrát alkalmaztuk: Minden cellának van egy UNDEFINED nev˝u flag-je, melynek igaz értéke esetén a cella GD értéke bizonytalan. Létezik továbbá egy FIFO lista (sor), amely azokat a cellákat tartalmazza, amelyek céltól való távolságát felül kell vizsgálni. A sor ügyel arra, hogy egy elem ne szerepeljen többször benne. LOOP: 1. Levesszük a legfels˝o elemet a sorból; ha nincs több elem, akkor kilépünk 2. Megvizsgáljuk a cella összes szomszédját, olyat keresve, amelyb˝ol biztos út vezet a célba – annak eldöntését, hogy létezik-e biztos út, egy egyszer˝u eljárással végezzük 3. Ha találtunk ilyen cellákat, akkor ezek közül a legkisebb GD+távolság érték˝ut megtesszük az aktuális cella célhoz vezet˝o következ˝o cellájának és e szerint módosítjuk GD értékét. Újfent megvizsgáljuk a cella összes szomszédját, és ha bármelyikre igaz a következ˝ok valamelyike, akkor fölvesszük a felülvizsgálandók sorába, és UNDEFINED flag-jüket beállítjuk: – GD értéke nagyobb, mint a jelenlegi cella GD értéke+ a távolságuk – következ˝o cellának a jelenlegi cella van megjelölve, és a cella GD értéke nem egyezik a jelenlegi cella GD érték+ a távolságukkal 4. Minden olyan szomszédos cellát fölveszünk a felülvizsgálandók sorába, amelyek UNDEFINED flag-je igaz érték˝u és a távolságuk kisebb végtelennél 4.3.2.4.

Tesztek

Mivel a képfeldolgozó rendszer még nem volt olyan állapotban, hogy valós képeket is tudjon produkálni, a tesztek során kénytelen voltam mesterségesen elkészített tesztképekkel/térképekkel dolgozni. A tesztek alatt úgy találtam, hogy az útvonal elkészítése olyan térképeken, amelyek véleményem szerint hasonló tulajdonságokkal rendelkeznek az objektumok s˝ur˝usége és elhelyezkedése szempontjából mint amilyeneket a képfeldolgozó rendszer fog generálni, szinte elhanyagolható id˝o alatt megtörténik. Lényegesen több id˝obe telik viszont olyan térképeken navigálni, amelyeken a legkisebb cellamérethez közelít˝o sz˝uk sikánokon keresztül vezet az út; ezeken az állapottereken akár több másodpercig is eltarthat, míg az eljárás eltalál a célig – ezt a kis méret˝u cellák nagyon nagy száma okozza. 42

A 4.8, 4.9 és 4.10 ábrákon látható teszteredmények egy AMD Athlon 1400XP processzoros, 256 Mbyte DDR memóriával ellátott gépen készültek 4 pixeles minimum és 64 pixeles maximum cellamérettel, a bal föls˝o sarokban lév˝o start és a jobb alsó sarokban lév˝o cél koordinátákkal.

4.9. ábra. Nagy bonyolultságú térkép: 49,67 ms

4.8. ábra. Várhatóan a valódihoz hasonló térkép: 3,58 ms

4.10. ábra. Sz˝uk folyosókat tartalmazó térkép: 3.520,50 ms

4.3.3. A generált út átalakítása Az út átalakítása során két f˝o szempontot kell figyelembe vennünk: azt, hogy a járm˝u nem képes bármely pozícióból bármely irányba továbbhaladni, továbbá, hogy nem kör alakú, és így nem mindegy, hogy egy pozícióban milyen irányultsággal helyezkedik el. Egy esetleges további szempont lehet, hogy a járm˝u els˝o kereke nem képes nulla id˝o alatt az általunk kívánt irányba állni, és így az alatt az id˝o alatt, amíg a kerék fordul, a kocsi a tervezett˝ol eltér˝o irányban halad. Ez utolsó szempontot jelenleg nem vesszük figyelembe az útvonal tervezése során, mivel 43

el˝oreláthatólag a kerék akkora sebességgel lesz képes irányba állni, hogy az az egyéb zavaró tényez˝okhöz viszonyítva (irányba állás pontatlansága, környezetr˝ol alkotott kép pontatlansága) elhanyagolható lesz, és az egyébként is mindenképpen szükséges sorozatos pályakorrekciók képesek lesznek kezelni ezt a problémát is. Az átalakításban a f˝oszerepet egy egyszer˝u algoritmus játssza, nevezzük "vontatórúd" algoritmusnak. Az eljárást a következ˝o módon lehet szemléltetni: A generált út elejére virtuálisan elhelyezzünk egy "free flying", vagyis egy szabadon szálló objektumot, amelyre jellemz˝o, hogy bármely irányban képes bármekkora mértékkel gyorsulni, így képes végighaladni az úton – ez lesz a vontató járm˝u. A szállító járm˝uvet tekintsük pótkocsinak, vagyis kössük össze a két els˝o kereket fixen egy tengellyel, majd ezt a tengelyt közepénél fogva függesszük fel a kocsira, mely felfüggesztésnél fogva változtatható a kerekek iránya . A tengelyre mer˝olegesen rögzítsünk fixen egy vontatórudat, majd ennek a rúdnak a végét kössük rá a vontatóra, itt már úgy, hogy az szabadon felvehessen minden irányt (4.11. ábra). Ha ezt a vontatót végigvezetjük az eredetileg generált úton, a vontatott járm˝u közelít˝oleg ugyanazt az utat fogja bejárni, mint a vontató, és így optimális esetben meg is oldottuk a vontatott járm˝u mozgásbeli megkötéseinek nem holonomikus volta miatti problémákat. (4.12. ábra).

4.11. ábra. Vontató és vontatandó összekapcsolása Az algoritmus a következ˝oképpen valósítottuk meg: a járm˝u els˝o kerekeinek középpontjának koordinátáiból rajzolunk egy rúdhossz sugarú kört, amelynek hi44

4.12. ábra. Átalakított út

bátlan esetben két helyen kell metszenie az út éppen aktuális szakaszából képzett egyenesét (az eredetileg generált út egyenes szakaszokból áll). Az egyik metszéspont induláskor a szakaszon kívül helyezkedik el, egyébként a szakasz már feldolgozott pontján, a másik pedig egy még feldolgozatlan ponton, vagy a szakasz másik végén túl. Amennyiben egyik metszéspont sem található feldolgozatlan területen, úgy a következ˝o szakaszra lépünk (amennyiben nincs több, úgy megállunk). Ha találtunk megfelel˝o metszéspontot, akkor a járm˝u els˝o kerekét a pont felé fordítjuk – ha ez túllépné a maximális fordulószöget, úgy a maximális értékre állítjuk be – majd a járm˝u els˝o kerekét hajtva a kívánt pontosságtól függ˝o egységnyit el˝oremozgunk (a járm˝u akkor tolat, ha ez a fordulószög ±180◦ maximális fordulószög tartományon belül van). Az eljárás végére kapunk egy, a járm˝u els˝o kerekeire vonatkozó beállítássorozatot, amit némi egyszer˝u sz˝urés után – ami a javított út nem egyenletes volta miatt kell – egy az egyben át tudunk adni a fizikai járm˝unek. A procedúra alkalmazásakor figyelembe kell venni azt a tényt, hogy a vontatott járm˝u nem pontosan az els˝o lépésben generált utat fogja bejárni – a rúd hosszától függ˝oen a sarkokat többé-kevésbé le fogja vágni, és szintén a rúd hosszától függ˝oen esetleg tolatásokat is végez – így szükséges a második lépésben generált út korrigálása amennyiben az tereptárgyat érint, vagy rossz esetben akár keresztül is megy rajta (4.13. ábra). A jól megválasztott rúdhossz sokat segíthet a probléma megoldásánál, de optimális rúdhosszt nehéz találni: túl kicsi hossz esetén a 45

járm˝u sokat fog tolatni – ez kerülend˝o -, túl nagy hossz esetén pedig nagy mértékben le fogja vágni a kanyarokat, és az is el˝ofordulhat, hogy egyes szakaszokra túl hosszú, más szakaszokra túl rövid méretet választottunk. Kiegészít˝o megoldásként szóba jöhet a váltakozó rúdméret: azokban a kanyarokban, amelyekben a járm˝u tolatásokra kényszerül, ezek száma csökkenthet˝o a hossz bizonyos heurisztika szerinti növelésével, túl nagy kanyarlevágás miatti ütközés esetén pedig meg lehet próbálni addig rövidíteni a rudat, amíg a járm˝u már nem érinti a tereptárgyat. Ugyanakkor ez a módszer nem ad megoldást arra az esetre, ha az eredeti út az egyik oldalról túl közel halad egy tereptárgyhoz, míg esetleg a másik oldalról b˝oven lenne hely: forduláskor a járm˝u mindenképpen elhagyja az utat bizonyos mértékben, ezért nekiütközik a tereptárgynak.

4.13. ábra. Az algoritmus hibájából adódó ütközések Egy jelenleg tesztelt módszer szép eredményekkel kecsegtet. A módszer lényege, hogy a vontatott járm˝u által generált utat megvizsgálva az eredetit módosítja úgy, hogy ezen az úton végighúzva a vontató-vontatott párost már kiküszöböl˝odjön a hiba, vagyis az ütközés. Az eljárás megvizsgálja, hogy a járm˝u hol lép be és hol hagyja el a kérdéses objektumot, majd ezt a két pontot összeköti egy egyenessel, az egyenest pedig addig tolja az eredeti út irányába, amíg az el nem hagyja az objektumot, majd módosítja az eredeti utat úgy, hogy a járm˝u ezen az egyenesen haladjon keresztül. Ha a járm˝u az újonnan generált szakaszon is nekiütközik valamely tereptárgynak, akkor az nagy valószín˝uséggel azt jelenti, hogy 46

itt az út túl sz˝uk, nem fér el a kocsi. Ebben az esetben új utat kell generálni ezen információ figyelembevételével. Az eljárás m˝uködéséhez szükséges, hogy az egyes tereptárgyakat azonosítókkal lássuk el. Ha ezt nem tennénk, akkor nehezen lehetne megállapítani, hogy ha találunk egy pontot, ahol a járm˝u épp befelé halad egy tereptárgyba és egy másikat, ahol elhagy egyet, akkor a két tereptárgy megegyezik vagy két különböz˝or˝ol van szó; körülményes lenne egy olyan algoritmus megvalósítása is, amely a belépési-kilépési pontokat összeköt˝o egyenest tolja az objektumon kívülre. Az azonosítást megvalósító algoritmus a következ˝o módon m˝uködik: Nulladik lépésként létrehozunk egy táblázatot, amely minden objektumazonosítóhoz hozzárendeli vagy önmagát vagy azt az objektumazonosítót, amelyhez a jelenlegi objektum tartozik. Ezek után sorra vesszük az azonosítandó objektumokat tartalmazó bináris kép pixeleit, és amennyiben olyanra akadunk, amely objektumot jelöl, akkor megvizsgáljuk az el˝otte és a fölötte lév˝o pixeleket: ha mindkett˝o üres, akkor ez a pixel egy teljesen új objektumazonosítót kap. Ha az el˝otte vagy a fölötte lév˝o pixelek közül pontosan egy objektum, akkor ennek az azonosítóját kapja meg. Ha a fölötte és az el˝otte lév˝o is objektum, akkor a fölötte lév˝o azonosítóját kapja meg, továbbá mivel az el˝otte és a fölötte lév˝o pixelek egy objektumhoz tartoznak, ezért hogy ne kelljen visszamenni a képen, a táblázatban följegyezzük az el˝otte lév˝o objektum azonosítójához a fölötte lév˝o objektum azonosítóját, majd az eljárás végeztével a táblázatban lév˝o átirányításokat földolgozzuk.

47

5. fejezet Eredmények, célkituzések ˝ Az eddigi munka során a képfeldolgozó modulban sikerült megoldani a két webkamera párhuzamos, valósidej˝u kezelését DirectShow segítségével. A kamerából nyert képekb˝ol klasszikus sarokpont kiemel˝o módszerrel tokenkiemelést valósítottunk meg. A két kép tokenpárjainak 80−90%-át sikerült megtalálni, a párosítás 3 − 5%-a volt hibás. A pontpárok pixel diszparitás értéke, és a kamerakalibráció során meghatározott kalibrációs görbék segítségével meg tudjuk határozni a pontok valós pozícióját (miliméterben mérve) a kamera koordinátarendszerében. Az így nyert háromdimenziós koordináták kell˝o mennyiség˝u és pontosságú adatot szolgáltatnak a modellkezel˝o modul számára. A modellkezel˝o modul képes a környezet modelljét felépíteni és javítani – az esetleges hibák kisz˝urésével együtt –, valamint a tárolt modell és a képfeldolgozó modultól kapott felület alapján az aktuális kamerapozíciót meghatározni. A rendszer a kamerapozíciót el˝oször az ICP algoritmussal próbálja meghatározni, ha ez sikertelen, akkor a spin-image algoritmussal meghatároz egy kis pontosságú pozíciót, majd ezt tovább finomítja az ICP algoritmussal. Elkészült a három dimenziós modellt két dimenziós térképekre konvertáló algoritmus, amely képes figyelembe venni az egymás fölé nyúló útvonalakat is - például egy asztalra fölvezet˝o rámpát. A parti-game algoritmusra alapozva sikerült megvalósítani egy közelít˝o cellafelbontást alkalmazó útkeres˝o rendszert, amely megfelel˝o sebességgel képes meghatározni két pont között az aktuális felbontáshoz tartozó legrövidebb utat. Általános esetben az eljárás néhány milliszekundum alatt lefut, nagy bonyolultságú térképek esetén is csak pár századmásodpercre van szüksége, az egyedüli problémát azon esetek jelentik, amikor a járm˝unek sz˝uk, hosszú folyosókon kell áthaladnia. Implementálásra került továbbá az el˝oz˝oekben meghatározott útvonalat a hordozójárm˝u által járhatóvá tév˝o algoritmus is, amely többek között korrigálja az eredeti útvonalban lév˝o, átmenet nélküli irányváltásokat. Utóbbi eljárás a járm˝u fizikai irányításához közvetlenül alkalmazható 48

adatokat produkál. További terveinkben szerepel a monogenikus jelre épül˝o képfeldolgozó módszer implementálása, illetve a sztereó képpárok vizsgálata mélységdiagramokkal. A modellkezel˝o modul javítása oly módon, hogy a környezet változásait érzékelje a modell építése során. Mivel a spin-image módszer nagyon érzékeny a normálvektorokban fellép˝o zajokra, tervezzük ezen algoritmus javítását. További teend˝o az irányítórendszer valós környezetbe való átültetése, a D* keresés megvalósítása a navigációs rendszer esetleges gyorsítása érdekében, illetve a környezet Framed Quadtree alapú reprezentációjának tanulmányozása.

49

Irodalomjegyzék [1] M. A. Al-Ansari and R. J. Williams. Modifying the parti-game algorithm for increased robustness, higher efficiency, and better policies. In Proceedings of the Tenth Yale Workshop on Adaptive and Learning Systems, pages 204–209, June 1998. New Haven, CT. [2] Sanjiv Singh Alex Yahja, Anthony Stentz and Barry L. Brumitt. Framedquadtree path planning for mobile robots operating in sparse environments. In IEEE Conference on Robotics and Automation, (ICRA), Leuven, Belgium, May 1998. [3] Dana Harry Ballard and Christopher M. Brown. Computer Vision. Prentice Hall Professional Technical Reference, 1982. [4] Paul J. Besl and Neil D. McKay. A Method for Registration of 3-D Shapes. IEEE Trans. Pattern Anal. Mach. Intell., 14(2):239–256, 1992. [5] The Boost.Python Library. python/doc/index.html.

http://www.boost.org/libs/

[6] M. Felsberg and G. Sommer. The Monogenic Signal. Technical Report 2016, Institute of Computer Science and Applied Mathematics, ChristianAlbrechts-University of Kiel, Germany, May 2001. Available at http:// www.ks.informatik.uni-kiel.de. [7] M. A. Fischler and R. C. Bolles. Random sample consensus: A paradigm for model fitting with applications to image analysis and automated cartograpy. Communications of the ACM, 24(6):381–395, June 1981. [8] B. K. P. Horn. Closed-form solution of absolute orientation using unit quaternions. Journal of the Optical Society of America A, 4(4):629–642, April 1987. [9] B. K. P. Horn, H. M. Hilden, and S. Negahdaripour. Closed-form solution of absolute orientation using orthonormal matrices. Journal of the Optical Society of America A, 5(7):1127–1135, July 1988. 50

[10] Andrew Johnson and Martial Hebert. Surface registration by matching oriented points. In International Conference on Recent Advances in 3-D Digital Imaging and Modeling, pages 121–128, May 1997. [11] N. Krüger, M. Felsberg, C. Gebken, and M. Pörksen. An explicit and compact coding of geometric and structural information applied to stereo processing. In Vision, Modeling, and Visualization, Erlangen, 2002. [12] Jean-Claude Latombe. Robot Motion Planning. Kluwer Academic Publishers, 1991. [13] The libkdtree++ library. Available at: http://cvs.ailab.ch/ cgi-bin/viewcvs.cgi/libkdtree++/. [14] The MAXIMA Computer Algebra System. maxima.sourceforge.net.

Available at: http://

[15] Ross J. Micheals and Terrance E. Boult. A new closed-form approach to the absolute orientation problem, 1999. Master’s thesis, Lehigh University. [16] Andrew W. Moore. The parti-game algorithm for variable resolution reinforcement learning in multidimensional state-spaces. In Jack D. Cowan, Gerald Tesauro, and Joshua Alspector, editors, Advances in Neural Information Processing Systems, volume 6, pages 711–718. Morgan Kaufmann Publishers, Inc., 1994. [17] Andrew W. Moore and C. G. Atkeson. The parti-game algorithm for variable resolution reinforcement learning in multidimensional state-spaces. In Machine Learning 21, 1995. [18] Open source computer vision system. opencvlibrary.sourceforge.net.

Available at: http://

[19] http://www.fuzzgun.btinternet.co.uk/rodney/vision.htm. [20] S.M. Smith and J.M. Brady. SUSAN - a new approach to low level image processing. Int. Journal of Computer Vision, 23(1):45–78, May 1997. [21] Anthony Stentz. Optimal and efficient path planning for partially-known environments. Technical report, The Robotics Institute; Carnegie Mellon University; Pittsburgh, PA 15213, 1994. [22] Anthony Stentz. The focussed d* algorithm for real-time replanning. Technical report, Robotics Institute; Carnegie Mellon University; Pittsburgh, Pennsylvania 15213, 1995. 51