designspeci kation · 2011. 10. 10. · imap lith 7 oktober 2011 designspeci kation emanuel w...

iMapLiTH

7 oktober 2011

DesignspecifikationEmanuel W Viklund

Version 1.0

StatusGranskadGodkänd Jonas Callmer 2011-10-04

TSRT10 CDIO-projektDesignspecifikation

Autonom bandvagn1

LIPs

iMapLiTH

7 oktober 2011

iMapNamn Ansvar Telefon E-postHanna Pettersson Projektledare (PL) 0705-402439 [email protected] Nilsson Informationsansva-

rig (IA)0735-081192 [email protected]

Daniel Karlsson Testansvarig (TA) 0708-378337 [email protected] Arvidsson Dokumentansvarig

(DOK)0709-620729 [email protected]

Emanuel WViklund

Designansvarig(DA)

0730-931087 [email protected]

Johan Wågberg - 0730-464739 [email protected] Bernhard - 0768-199611 [email protected]

Epostlista för hela gruppen: [email protected]: http://www.isy.liu.se/edu/projekt/reglerteknik/2011/bandvagn

Beställare: Jonas Callmer, Linköpings universitet, 013 - 28 40 58, [email protected]: Malin Ingerhed, Saab Dynamics, 013 - 186383, [email protected]

Kursansvarig: David Törnqvist, Linköpings universitet, 013 - 28 18 82 ,[email protected]

Handledare: Johan Dahlin, Linköpings universitet, 013 - 28 23 06,[email protected]


Autonom bandvagn2

LIPs

iMapLiTH

7 oktober 2011

Dokumenthistorik

Version Datum Utförda förändringar Utfört av Granskad av

0.1 2011-09-26 Första utkast Emanuel Daniel0.2 2011-09-28 Spr̊akliga ändringar och

utvecklatpositioneringsavsnitt

Jacob

0.3 2011-10-03 Ändringar enligtkommentarer fr̊anbeställare och handledare

Alla

1.0 2011-10-07 Version 1.0 Marcus Daniel


Autonom bandvagn3

LIPs

INNEHÅLL iMapLiTH

7 oktober 2011

Inneh̊all1 Inledning 5

2 Systemöversikt 52.1 Grov systembeskrivning . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62.2 Ing̊aende delsystem . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

3 Masterenheten 73.1 Kartering . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

3.1.1 Bibliotek . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 83.1.2 Skapa djupbild . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

3.1.2.1 ELAS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 83.1.3 Generera punktmoln . . . . . . . . . . . . . . . . . . . . . . . . . . 93.1.4 Ihopsättning av punktmoln . . . . . . . . . . . . . . . . . . . . . . 93.1.5 Skapa polygoner . . . . . . . . . . . . . . . . . . . . . . . . . . . . 103.1.6 Städa polygonvärld . . . . . . . . . . . . . . . . . . . . . . . . . . . 113.1.7 Texturera polygoner . . . . . . . . . . . . . . . . . . . . . . . . . . 113.1.8 Dataobjekt fr̊an kartering . . . . . . . . . . . . . . . . . . . . . . . 12

3.2 Användargränssnitt . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 123.2.1 Tidigare funktionalitet . . . . . . . . . . . . . . . . . . . . . . . . . 123.2.2 Vidareutveckling . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14

3.3 3D-världen . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 153.3.1 Bibliotek . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 163.3.2 Kravuppföljning . . . . . . . . . . . . . . . . . . . . . . . . . . . . 163.3.3 Texturering . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17

3.4 Ruttplanering . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 173.4.1 Förenklingar . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 173.4.2 Exempel p̊a algoritmstruktur för diskret modell . . . . . . . . . . . 18

4 Scoutenheten 184.1 Positionering . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19

4.1.1 Bibliotek . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 194.1.2 Visuell odometri . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19

4.1.2.1 Hitta punkter . . . . . . . . . . . . . . . . . . . . . . . . 204.1.2.2 Matcha punkter . . . . . . . . . . . . . . . . . . . . . . . 214.1.2.3 Behandling av outliers . . . . . . . . . . . . . . . . . . . . 21

4.1.3 GPS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 214.1.4 Odometer . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 214.1.5 Fusionering . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22

4.1.5.1 Fr̊an globala koordinater till pixelkoordinater . . . . . . . 224.1.5.2 Mätuppdatering . . . . . . . . . . . . . . . . . . . . . . . 234.1.5.3 Dataobjekt . . . . . . . . . . . . . . . . . . . . . . . . . . 24

4.2 Kollisionshantering . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 244.2.1 Kollisionshantering för hinder . . . . . . . . . . . . . . . . . . . . . 244.2.2 Kollisionshantering för förbjudna omr̊aden . . . . . . . . . . . . . . 25

A Konventioner 26A.1 Dokumentkonventioner . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26A.2 Kodkonventioner . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26


Autonom bandvagn4

LIPs

iMapLiTH

7 oktober 2011

1 Inledning

Saab Dynamics vill utveckla en bandvagn som ska kunna kartlägga olika miljöer autonomt.Ett flertal projekt har genomförts för att uppn̊a m̊alet. Ett första projekt genomfördesv̊aren 2009 av projektgruppen O’hara’s med utveckling av specifikationer, kontrollpro-gram, nätverkskommunikation, navigationstekniker med mera. Under hösten 2009 fort-satte gruppen Carpe Locus och utrustade bandvagnen med fjärrstyrning samt motorre-glering för framdrivning. Bandvagnen utrustades även med en GPS för att kunna följaen fördefinierad brytpunktsbana. Senaste projektet gjordes hösten 2010 av gruppen 8Yaresom förde över tidigare funktionalitet till en industridator och installerade en stereokameraav typen Bumblebee 2.

Målet för detta projekt är att utöka bandvagnens funktionalitet genom att med hjälpav kamerans stereobilder och sensorer bygga upp en 3D-karta av ett omr̊ade p̊a upp till100x100 meters storlek.

Detta dokument behandlar hur systemet ska designas för att uppn̊a de nya krav somställts p̊a det. Utöver den nya funktionalitet som ska implementeras kommer även en delav de tidigare gruppernas arbete behöva göras om eller förbättras.

2 Systemöversikt

Systemet är uppbyggt av tv̊a delsystem, masterenheten och scoutenheten. Masterenhetenanvänds för att kontrollera och övervaka scoutenheten. I Figur 1 ses en översiktlig bild överhur systemet är uppbyggt, där scoutenheten definieras av bandvagnen och dess tillhörandedelsystem.

Figur 1: Översikt av systemet


Autonom bandvagn5

LIPs

2.1 Grov systembeskrivning iMapLiTH

7 oktober 2011

2.1 Grov systembeskrivning

Det första delsystemet (masterenheten) är en laptop som används för att styra bandvag-nen. Det andra delsystemet (scoutenheten) är själva bandvagnen. Dessa tv̊a delsystemkommunicerar med varandra via ett tr̊adlöst nätverk (WLAN). Scoutenheten är i sin turuppbyggd av tv̊a delsystem: navigationsenheten och framdrivningsenheten ARM. De sen-sorer som ing̊ar är en GPS och en stereokamera som är kopplad till navigationsenhetenoch tv̊a odometrar som är kopplade till ARM-processorn.

I Figur 2 visas informationsflödet inom systemet. Det tidigare systemet utökas för attutföra kartering, dvs. uppbyggnad av punktmolnsvärld och polygonvärld, samt för attnyttja visuell odometri.

AnvändargränssnittModul för

bildbehandling och kartering

Nätverksmodul

Nätverksmodul

Sensormodul

Rörelsemodell

Mobilitetsmodul

GPS

Odometrar

Stereokamera

Masterenhet

Scoutenhet

Images, Polygon World, Point Cloud

ImageData

CameraData

StatusGPSData

WheelDataOdometryData

CameraSettingsAutomatic

Manual

CameraDataStatus

GPSDataWheelData

OdometryData

ImageData

CameraSettingsAutomatic

Manual

CameraData

CameraSettings

GPSData

WheelData

GPSDataWheelData

OdometryDataCameraData

Status

CameraSettings ManualAutomaticImageData

WheelDataGPSData

OdometryData

Status

Figur 2: Informationsflöde inom systemet

Nedan följer en kort beskrivning p̊a vad de olika modulerna gör.

AnvändargränssnittModulen hanterar uppritandet av det grafiska, samt möjliggör kommunikation mel-lan användare och system.

Modul för bildbehandling och karteringModulen sköter karteringen och all bildbehandling som behövs till användargräns-snittet.


Autonom bandvagn6

LIPs

2.2 Ing̊aende delsystem iMapLiTH

7 oktober 2011

NätverksmodulModulen sköter kommunikationen mellan masterenheten och scoutenheten.

SensormodulModulen samlar ihop all sensordata och gör en första bearbetning av denna. Sensor-datat kommer kunna användas av exempelvis mobilitetsmodulen för att positionerasig.

MobilitetsmodulModulen sköter dels förflyttningen utav scoutenheten utifr̊an styrkommandon fr̊anmasterenheten samt sensordata, dels bearbetning av sensordata för skattning avposition.

RörelsemodellVia rörelsemodellen f̊ar sensormodulen nödvändig information om hur scoutenhetenrör sig, vilket kan förbättra bearbetning av sensordata.

2.2 Ing̊aende delsystem

De ing̊aende delsystemen är:

• Masterenheten

• Scoutenheten– Navigationsenheten

– Framdrivningsenheten ARM

3 Masterenheten

Masterenheten utgörs av en laptop, där ett användargränssnitt körs under ett Linux-baserat operativsystem. Fr̊an användargränssnittet skickas kommandon till bandvagnen(scoutenheten). Bandvagnen kan endera köras i autonomt eller manuellt läge. I autonomtläge skickas en av användaren specificerad brytpunktsbana till bandvagnen. Bandvagnenföljer sedan denna bana s̊a bra som möjligt. I manuellt läge skickar användaren styrkom-mandon direkt till bandvagnen.

Figur 3: Masterenheten kommunicerar med scoutenheten via WLAN.

3.1 Kartering

Det som i detta dokument kallas kartering kallas i litteraturen ofta för Surface recon-struction, Structure from motion eller 3D reconstruction se till exempel [11] och [10].


Autonom bandvagn7

LIPs

3.1 Kartering iMapLiTH

7 oktober 2011

Många algoritmer finns implementerade men alla löser i stort sett samma delproblem p̊avägen fram till den slutliga kartan. Nedan beskrivs en väg att skapa en representation avomvärlden utifr̊an stereobilder. Många av de delproblem som m̊aste lösas har flera olikalösningar. Avgörande för vilken metod som är bäst är beräkningstiden i jämförelse medresultatets kvalitet. Många lösningsmetoder har hög beräkningskapacitet m.a.p. antaletanalyserade stereobilder. Detta m̊aste utvärderas utifr̊an verkliga bilder fr̊an kameran. Enöversiktlig bild över de olika stegen i framtagandet av kartan visas i Figur 4.

Ta in nytt stereobildpar

Skatta djup

Fusionera nytt punktmoln med

befintligt

Bilda punktmoln

Första punktmolnet?

nej

ja

Position och orientering

Figur 4: Översiktlig bild av stegen i bildandet av 3D-kartan.

3.1.1 Bibliotek

Flertalet bibliotek för hantering av bilder finns att hämta gratis p̊a internet. Till exempelfinns Cimg som implementerar primitiver för att lagra och visa bilder. Ett annat kraftfulltbibliotek är Cgal. Cgal implementerar m̊anga funktioner i C++ för beräkningar inomgeometri. Många, om inte alla, av de delmoment som krävs vid karteringen, kan användafunktioner ur Cgals bibliotek.

3.1.2 Skapa djupbild

Ett första steg mot en 3D-karta är att utifr̊an det senaste stereobildparet skatta en djup-bild, det vill säga en bild där avst̊andet fr̊an kameran till omvärlden skattas för varjepixel i bilden. Den algoritm som idag finns implementerad av tidigare projektgrupper gertroligen en för gles bild där det inte g̊ar att bestämma n̊agot djup för allt för m̊angapixlar. En bättre metod som kallas ELAS [6] (Efficient LArge-scale Stereo). Den kan geen tät djupbild där djupet för nästan alla pixlar kan skattas i b̊ada ursprungsbilderna ochsamtidigt ge en snabb beräkningstid. Algoritmen är avancerad men finns implementeradi det fria biblioteket libelas [5].

3.1.2.1 ELAS

För att generera en tät djupbild över hela bilden, skattar ELAS djupet i tv̊a steg. Förstidentifieras ett mindre antal punkter som är enkla att känna igen för att p̊a s̊a sätt


Autonom bandvagn8

LIPs


7 oktober 2011

minimera risken för felmatchningar mellan bilderna. Djupet för dessa punkter skattasdärefter med hjälp av den kända kamerageometrin och punkternas pixelkoordinater irespektive bild. Utifr̊an dessa ”säkra” punkter bildas sedan en a priori-fördelning fördjupet i varje pixel i b̊ada bilderna. Detta sker genom en triangularisering av 2D-bildenutifr̊an de matchade punkternas pixelkoordinater där djupet i varje triangel bildar ettplan utifr̊an djupet i dess hörnpunkter. Djupet i hela bilden skattas allts̊a som en styckvislinjär funktion med stöd i redan beräknade punkterna. Utifr̊an denna a priori-fördelningkan sedan djupet i resterande pixlar beräknas genom en maximum a postiori skattning.

3.1.3 Generera punktmoln

Utifr̊an den genererade djupbilden kan ett punktmoln skapas där varje punkt placeras ut iett koordinatsystem fixt i omvärlden i vilket bandvagnen rör sig. För att göra detta krävsdels djupbilden men även kamerans position och orientering i det fixa koordinatsystemetd̊a kortet tagits samt en modell av kameran. Genom att utg̊a fr̊an kamerans position kankameramodellen användas för att baklänges räkna fram en punkt i det fixa koordinat-systemet där det borde finnas ett objekt. Beroende p̊a hur skattningen görs kan ävenett konfidensintervall beräknas för punkten. Detta skulle användas för att s̊alla bort alltför osäkra punkter. Senare borde det även g̊a att användas detta d̊a det nya punktmol-net skall sammanfogas med det sedan tidigare erh̊allna punktmolnet för att ge en bättresammanvägning mellan säkra och osäkra punkter..

Utöver positionen hos punkten skall även en del övrig information sparas. För det förstaska en färg sättas till punkten genom att spara den färg som finns i bilden för den specifikapixeln. Dessutom ska ett index som beskriver vilken bild punkten har skapats ifr̊an sparas.Denna information används senare d̊a 3D-världen ska textureras.

Ett alternativ till att beräkna punktmolnet utifr̊an den täta djupbilden är att användasamma teknik som kommer att användas för den visuell odometrin. Där identifierasutmärkande punkter i bilden med hjälp av algoritmerna SIFT [9] eller SURF [1]. Me-toderna användas för att identifiera samma punkt i tv̊a olika bilder. Matchas punkternaihop i de tv̊a bilderna fr̊an ett stereobildpar kan punktens världskoordinater skattas ut-ifr̊an den kända kamerageometrin. Dessa punkter kan sedan användas i punktmolnet.Detta bör ge en snabbare beräkningstid och kanske en enklare algoritm, men i gengäldkommer punktmolnet inte inneh̊alla lika m̊anga punkter.

3.1.4 Ihopsättning av punktmoln

Efter att ett nytt punktmoln har skattats, ska det vägas samman med det gamla. P̊a dettasätt byggs världen upp med mer och mer information. Det enklaste sättet att hantera dettap̊a är att lägga till de nya punkterna och spara alla punkter som skattats under körningen.Detta fungerar väl för sm̊a omr̊aden med ett litet antal bilder men när omr̊adet blir störreoch antalet bilder ökar kommer denna metod att kräva mycket minne. Dessutom erh̊allsingen förbättring av kartering av objekt som observeras flera g̊anger.

Problemet att fusionera punktmoln brukar i litteraturen ofta kallas bundle adjustment[12]. Bäst resultat erh̊alls genom att minimera en olinjär förlustfunktion över punktmolnfr̊an alla djupbilder, men detta kräver att även det s̊a kallade associationsproblemet är löst.Det vill säga att punkter i punktmolnet som härstammar fr̊an samma punkt i verklighetenhar identifierats. Används tekniken att bilda punktmoln utifr̊an en tät djupbild kan dettainte göras. Detta är dock möjligt ifall punktmolnen skattas med hjälp av n̊agon av SIFTeller SURF algoritmerna. En annan metod för att lösa detta problem föresl̊as av Geiger


Autonom bandvagn9

LIPs


7 oktober 2011

et al. i [7]. Idén g̊ar ut p̊a att projicera det gamla punktmolnet p̊a den nya djupbildenoch för varje punkt kontrollera om denna hamnar inom ett giltigt djupintervall. Är dettafallet vägs punkterna samman genom deras medelvärde, annars betraktas punkten somny. Ett flödesschema över detta visas i Figur 5. Geiger visar i sin rapport att detta gersnabba beräkningar och ett tillräckligt bra resultat. P̊a samma sätt kan även färgen vägassamman som ett medelvärde. Slutligen utökas listan med vilka bilder som punkten harobserverats i.

Ta in nytt punktmoln

Projicera gammalt punktmoln på aktuell

kameraposition.

Jämför den projicerade punktens djup med

senaste djupbild

Punktens djup inom

given tolerans?

Inför punkten som en ny

punkt i molnet

Väg samman de två punkterna med

deras medelvärde

Alla punkter gjorda?

Klart

Ordna punkterna i en given ordning. Välj den

första.

Välj nästa punkt i listan

ja

nej

nej

ja

Figur 5: Flödesschema för fusionen av ett nytt punktmoln med det gamla.

3.1.5 Skapa polygoner

Utifr̊an det skapade punktmolnet skapas en 3D-värld av polygoner. Polygonerna gene-reras genom Delaunay-triangularisering med hjälp av Cgal-biblioteket. Denna algoritm


Autonom bandvagn10

LIPs


7 oktober 2011

spänner upp trianglar s̊a att inga punkter ligger inuti n̊agon triangels omskrivna sfär, p̊asamma g̊ang som smala trianglar undviks [2].

Genom att studera vilka bildpar som gav upphov till polygonens ing̊aende punkter, kanett visst bildpar väljas som källa till polygonen. I det fall att alla polygonens punkterhärstammar fr̊an samma bildpar st̊ar det klart att detta skall användas vid textureringsamt städning. D̊a ett bildpar tas nära eller vinkelrätt mot en viss yta kommer punkternai punktmolnsvärlden om härstammar fr̊an dessa bilder att bli tätare än de fr̊an bildpartagna l̊angt bort ifr̊an eller fr̊an sned vinkel. Sannolikheten att majoriteten av en poten-tiell polygons punkter kommer fr̊an det bildpar som har bäst betraktelseförh̊allande, dvs.indexeras till detta, blir d̊a större än för de bildpar med sämre förh̊allande.

Punkterna polygonen ursprungligen spänns upp av inneh̊aller även färginformation. Ettsnitt av deras färginformation kan användas för att bestämma en specifik färg till var-je polygon. Betraktas även punkternas osäkerhet kan färginformationen även viktas vidsammanvägningen; st̊ar det klart att en viss punkt med stor sannolikhet inte g̊ar att litap̊a gällande exempelvis färg kan den i större grad bortses ifr̊an.

3.1.6 Städa polygonvärld

Ett problem som uppst̊ar d̊a punktmolnet triangulariseras är att felaktiga och överflödigapolygoner skapas. För att göra poygonvärlden mer tilltalande m̊aste den rensas. Dettasker genom att jämföra polygonernas position gentemot de djupbilder de sägs tillhöra.Vid jämförelse mellan en djupbild och de polygoner som är indexerade till just den djup-bilden kan man detektera om n̊agon polygon spänts upp framför det djup som djupbildenvisar. Om s̊a är fallet ska den polygonen raderas. Genom att endast jämföra med de fördjupbilden tillhörande polygonerna erh̊alls att ingen polygon borde dölja en fr̊an djup-bilden teoretisk punkt mer än ett visst tröskelvärde. En del i städningsproblemet är atthitta det tröskelvärde som ger bäst resultat.

Vid städning av polygonvärlden bestäms även varje polygons normal. Genom att numrerade ing̊aende punkterna, sett fr̊an kameran, kommer det grafiska gränssnittet tolka det somatt polygonens normal pekar mot kameran. Vid vetskap om åt vilket h̊all varje yta vändersig kan uppritning i 3D snabbas upp, samt grafiska effekter som skuggor och reflektionerenkelt beräknas.

Normalriktningen erh̊alls genom kryssprodukten mellan tv̊a vektorer fr̊an polygonensförsta punkt med var och en av de tv̊a övriga. Polygonerna för en given bild ska varavända mot kameran; är en polygon uppspänd fr̊an en viss bild ska den även synas fr̊ansamma position i 3D-världen. D̊a normalen för en polygon ej är i rätt riktning givet enviss bild numreras punkterna om.

3.1.7 Texturera polygoner

Texturering sker genom att projicera varje polygon p̊a den bild polygonen är indexeradtill. Polygonens hörn i 3D kommer att f̊a relaterade koordinater i 2D-bilden, som sedemerakan användas vid uppritning av världen.

Projiceringen sker genom att beskriva polygonernas koordinater, {xp, yp, zp}, i koordi-natsystemet med kamerans position som origo, riktat åt det h̊all bilden togs. Titth̊alska-meramodell kan d̊a användas, och givet koordinaterna, {xk, yk, zk}, i detta system samt


Autonom bandvagn11

LIPs

3.2 Användargränssnitt iMapLiTH

7 oktober 2011

fokaldjupet, f , ges koordinaterna, ui, i bilden som

{ux, uy} ={fxkzk, fykzk

}. (1)

3.1.8 Dataobjekt fr̊an kartering

Efter karteringen erh̊alls en polygonvärld som skapas ur de punkter som tillsammansbildar punktmoln. Här följer beskrivning av de ing̊aende objekten och deras viktigastedatamedlemmar. Namnen är givna p̊a engelska enligt gällande kodkonvention. Bildindexbetyder i detta fall det bildpar som en punkt eller polygon sägs tillhöra.

PointEn punkt best̊aende av koordinater, färg, bildindex och kovarians.

pointCoordinates Array med punkens koordinater [x, y, z].

pointColor Array med punktens färgkod [R, G, B].

imageIndex Punktens bildindex.

pointCovar Osäkerhet i punktens position.

Point CloudHanterar lista med Points.

listOfPoints Array med punktvärldens alla Points.

PolygonEn triangel best̊aende av punkter, färg, bildindex och relaterade koordinater i bil-den.

polygonPoints Array med polygonens tre Points.

polygonColor Array med polygonens färgkod [x, y, z].

imageIndex Polygonens bildindex.

imagePolygonCoordinates Flerdimensionell array med varje punkts motsva-rande koordinat i bilden [[x1, y1],[x2, y2], [x3, y3]].

Polygon WorldEn lista med Polygons.

listOfPolygons Array med polygonvärldens alla Polygons.

3.2 Användargränssnitt

Det användargränssnitt (GUI) som används p̊a masterenheten har utvecklats av tidi-gare projektgrupper(8Yare, O’hara’s och Carpe Locus). Användaren kan med hjälp avanvändargränssnittet ansluta till bandvagnen och därefter skicka och ta emot data samtpresentera denna p̊a olika sätt. Programmet är utvecklat i C++ och använder Qt-biblioteketför det grafiska gränssnittet.

3.2.1 Tidigare funktionalitet

Vid start av programmet möts användaren av ett anslutningsfönster där IP-adressen tillnavigationsenheten skrivs in. Efter att anslutningen är upprättad kan styrkommandonskickas till roboten. Dessutom kan en brytpunktsbana definieras i användargränssnittetoch överföras till roboten som d̊a utför förflyttning efter banan.


Autonom bandvagn12

LIPs


7 oktober 2011

I manuellt läge skickas styrkommandon direkt till roboten. Användaren kan via ett has-tighetsreglage ställa in önskad hastighet och med hjälp av tangentbordet styra roboten.Användaren kan även välja vilken data fr̊an roboten som ska visas i användargränssnittet:GPS-data, odometerdata eller bilder fr̊an kameran. Kamerainformationen kan represente-ras som singel-bilder, stereobilder eller färglagda djupbilder. Det g̊ar även välja att följaett m̊al.

I autonomt läge följer roboten en bana given av ett antal brytpunkter. Den bana somroboten ska följa kan användaren själv bestäma genom att klicka p̊a skärmen s̊a att bryt-punkter placeras ut och därmed bildar ett sp̊ar att följa. Det g̊ar även att öppna en redanbefinlig bana eller att spara den nuvarande banan. Ett antal olika styrknappar finns föratt göra det möjligt att bestämma vad roboten ska göra och vilken information om Scou-tenheten som ska visas. Samma sensorinformation som för det manuella läget g̊ar ävenatt se i det autonoma.

Utöver dessa lägen finns ett satellitläge. Där är det möjligt att f̊a upp en satellitbild överomr̊adet roboten ska köra i. För tillfället existerar bara en karta i programmet och det ären bild över och omkring B-huset p̊a Linköpings universitet. I denna vy g̊ar det som i denautonoma menyn att ladda in en brytpunktsbana eller att skapa en egen.

Klasser som redan finns och vilken funktion de har kan ses i Tabell 1 samt ett klassdiagramkan ses i Figur 6.

Klassnamn BeskrivningUserInterface Skapar huvudfönstretGLWidget Huvudklass för att rita ut satellitkartanAutomaticGUI Gränssnitt för automatisk styrningManualGUI Gränssnitt för manuell styrningNetworkGUI Skapar fönster för nätverksanslutningSensorGUI Tar in och visar sensordataScoutData Sparar information om roboten s̊a som

position, GPS-data, bildpositioner ochbrytpunktskartan

SatellitePanel Visar knapparna i satellitvynLoadCameraPositionGUI Laddar in en bana fr̊an en textfilBreakGUI Sköter inmatning av brytpunktslista och

sparning av denTrackGUI Skapar ett fönster s̊a det g̊ar att ladda in en

förgenererad banaStatusGUI Visar statusinformationMapGUI Ritar ut robotens position relativt sin

startpunktTracker Klass för identifiering av rörliga objektCameraGUI Visar kamerabilder i olika versionerCameraSettingsGUI Sköter inställningar för kameranAboutUsGui Visar information om projektgruppen

Tabell 1: Beskrivning av befintliga klasser.


Autonom bandvagn13

LIPs


7 oktober 2011

3.2.2 Vidareutveckling

Användargränssnittet ska vidareutvecklas s̊a att det finns ett läge där användaren kan seoch röra sig i den 3D-värld som roboten har skapat efter karteringen. Världen ska kunnavara uppbyggd av antingen polygoner eller punktmoln. Växling mellan dessa metoder skakunna göras.

För att kunna markera förbjudna omr̊aden eller markera det omr̊ade som ska karteras ianvändargränsnittet ska det implementeras en Click-and-Drag funktion vilket det redanfinns stöd för i biblioteket Qt. En knapp för att välja vilket typ av omr̊ade som ska väljasska finnas.

När användargränssnittet utökas kommer den kodkonvention som projektgruppen CarpeLocus tagit fram att följas. Även objekthanteringen kommer följa den bestämda struktu-ren. De nya klasser/metoder som kommer att skapas är:

• Interface3d - Klass som ska hantera ett fönster som inneh̊aller en 3D-vy. Dennakommer vara nära kopplad till OpenGl-paketet och hanterar användarinteraktionb̊ade med 3D-världen och med gränssnittet.

• World3d - Klass som beskriver alla funktioner som kommer behöva användas avde olika OpenGL renderade världarna.

• PointWorld - Subklass till World3d. PointWorld inneh̊aller de funktioner som ärspecifika för punktmolnsvärlden.

• PolygonWorld - Subklass till World3d. PolygonWorld inneh̊aller de funktioner somär specifika för polygonvärlden.

En större del av existerande klasser och metoder fr̊an tidigare grupper kommer behövautökas med ytterligare funktionalitet. D̊a vissa filer är otillräckligt kommenterade kom-mer dessa att kompletteras i m̊an av tid för att hjälpa nästkommande grupper. De nyaklassernas relationer kan ses i Figur 6.


Autonom bandvagn14

LIPs

3.3 3D-världen iMapLiTH

7 oktober 2011

Figur 6: Klassdiagram - Bl̊aa rutor är befintliga klasser och gröna är de somska skapas för detta projekt.

3.3 3D-världen

Utvecklingen av användargränssnittet är mycket beroende av resultatet fr̊an karteringen.För att tjäna in tid kan en annan enkel 3D-värld skapas där funktionerna för Interface3d-klassen kan testas. Den information som antas erh̊allas fr̊an karteringen beskrivs i avsnitt3.1.8. I detta avsnitt används ordet objekt som beskrivning av en geometrisk form somär uppbyggd av ett antal polygoner eller punkter.

3D-världen ska best̊a av antingen polygoner eller punkter. Detta val ska kunna göras avanvändaren i användargränssnittet.

När världen är uppbyggd av punkter benämns den som en punktmolnsvärld. I punkt-molnsvärlden ska punkterna representeras av sm̊a sfärer med en fast storlek. När en punktskapas ska den ges samma färg som den motsvarar i stereobilden och denna informationges fr̊an karteringsgruppen. För att f̊a en uppfattning om varifr̊an en bild är tagen, skadet finnas pilar som representerar detta strax ovanför marken i punktmolnsvärlden.

När världen är uppbyggd av polygoner benämns den som en polygonvärld. Fr̊an karte-ringsfasen erh̊alls en lista med polygoner som ska vara en del av polygonvärlden. D̊a pre-standan hos masterenheten kan vara begränsad är det viktigt att funktioner som ”frustumculling” (se nedan för förklaring) samt ”back-face culling” stöds och kan implementerasom behov finns. Därför antags polygonernas Points vara angivna i den ordning som gerpolygonen rätt normal. D̊a kan ”back-face culling” enkelt implementeras, vilket innebäratt alla polygoner som är vända bort fr̊an kameran ignoreras.

För att minska beräkningskraften för ”frustum culling” och kollisionshantering för OpenGL-kameran, är det viktigt dela in världen i mindre delar. OpenGL-kameran kan med fördelrepresenteras som en sfär för att förenkla kollisionshanteringen och förhindra att kamerantill̊ats komma för nära objekt. Istället för att behöva testa om kameran kolliderar med


Autonom bandvagn15

LIPs

3.3 3D-världen iMapLiTH

7 oktober 2011

varje polygon i hela världen, räcker det att undersöka i vilken del av världen kameranbefinner sig och endast testa kollisioner med de polygoner som är i samma världsdel. Hurstora delar världen ska splittas i undersöks senare i projektet.

”Frustum culling” innebär att man undersöker vilka objekt som kan ses av OpenGL-kameran och renderar endast dessa. För att det inte ska uppst̊a artefakter (fel i dengrafiska representationen) i utkanterna av vyn, kommer objekt runt om synfältet ocks̊aatt renderas. Objekt som är placerade i resten av världen kommer änd̊a inte att ses ochkan därför ignoreras.

3.3.1 Bibliotek

D̊a det tidigare programmet är utvecklat i C++, under Qt-biblioteket, är det logiskt attfortsätta att utnyttja Qt och därmed använda Qt:s inbyggda stöd för OpenGL. Dettainnebär att det inte behövs importera en OpenGL-instans utan istället ha den integreradi Qt. Det är viktigt att GLUT-paketet finns tillgängligt d̊a detta erbjuder en stor mängdförenklade funktioner som kommer vara till stor hjälp.

3.3.2 Kravuppföljning

För att möjliggöra för användaren att titta p̊a en specifik position kommer ett formulärfinnas i användargränssnittet där användaren kan ställa in önskad kamera position el-ler position som kameran ska titta p̊a. Om inget annat anges antas användaren börja ikoordinaterna (0,0,0) och titta i den riktning som roboten inledde sin färd.

För att möjliggöra att användaren ska kunna följa bandvagnens bana, ska informationenom bildernas position och vinkel användas. Banan ritas redan nu ut av gränssnittet ochgenom att göra om denna till en kamerabana, kan kameran ställas in att följa banan i mereller mindre mjuka rörelser.

Ett stöd för att hantera instruktioner fr̊an användaren via tangentbord och mus finnsredan i GLUT-paketet i OpenGL. Där kan varje tangent kopplas till en funktion ochp̊a detta sätt kan användaren röra sig omkring i 3D-världen. I nuläget är det tänkt attföljande rörelsekontroller ska finnas:

• w - rörelse fram̊at

• s - rörelse bak̊at

• a - rörelse rakt åt vänster

• d - rörelse rakt åt höger

• pil upp - titta gradvis upp̊at

• pil ner - titta gradvis ned̊at

• pil vänster - rotera runt sin egen axel åt vänster

• pil höger - rotera runt sin egen axel åt höger

Den 2D-karta som genereras är tänkt att vara ett genomskinligt färglager som kan placerasöver den översiktskarta som redan finns implementerad i gränssnittet. Den genomskinligaegenskapen ges genom att använda alpha-parametern i Qt-klassen QColor.


Autonom bandvagn16

LIPs

3.4 Ruttplanering iMapLiTH

7 oktober 2011

3.3.3 Texturering

Texturering är enbart möjligt i polygonvärlden d̊a punkterna i punktmolnsvärlden är obe-roende av varandra när de skickas till användargränssnittet. Dock borde det ge tillräckligtbra resultat med att endast tilldela varje punkt i punktmolnsvärlden en färg. Om punk-terna är tillräckligt täta borde detta ge samma effekt som en texturerad polygon i poly-gonvärlden.

För polygonvärlden kommer dock textureringen att medföra en stor förbättring av hel-hetsintrycket. Till varje polygon som erh̊alls fr̊an karteringen kommer det att anges vilkakoordinater i originalbilden som motsvaras. Inneh̊allet i originalbilden kan rättframt ex-traheras och mappas p̊a polygonen.

3.4 Ruttplanering

För att kunna hantera helt okända omgivningar är det önskvärt att roboten kan klara avatt kartera ett givet omr̊ade automatiskt. Detta kräver att roboten själv vet var bilderbehövs tas och hur den transporterar sig dit. För att kunna göra detta krävs en algo-ritm som beskriver vilka m̊al som ska uppn̊as, vilka alternativ som ges (e.g. förflytta sigfram̊at, svänga 90 grader åt höger, ta en bild) och vilken prioritetsordning som finns mel-lan dessa alternativ. Om en senare projektgrupp fokuserar mer p̊a detta omr̊ade, skulleantagligen stor vikt läggas p̊a att denna algoritm är relativt effektiv med avseende p̊a tideller information per tagen bild eller liknande. Dock är huvudm̊alet med ruttplaneringenför projektgrupp iMap, att endast skapa en fungerande algoritm som inte behöver varaeffektiv.

3.4.1 Förenklingar

Ett problem med ruttplaneringen är att man inte p̊a förhand vet exakt var hinder befinnersig. Detta innebär att förutom en bra positionering, är en fungerande kollisionshanteringviktig. Om vi antar att roboten kan detektera hinder och placera ut dessa p̊a den inter-na kartan är ruttplaneringsproblemet ett i stort sett iterativt, dynamiskt, kontinuerligtproblem, vilket gör det beräkningskrävande.

En förenkling kan vara att diskretisera de positioner och vinklar som man behöver tabilder i för att kartera ett omr̊ade. D̊a skulle det exempelvis räcka med att ta en bildvarje meter i fyra ortogonala riktningar. P̊a ett 100× 100m stort omr̊ade skulle det is̊afallkrävas 100×100×4 st bilder tagna av roboten. Genom att förenkla problemet ytterligaretex. genom att säga att: ’Har du tittat p̊a en yta utan hinder fr̊an EN riktning s̊a finnsinget hinder där fr̊an de övriga riktningarna’ eller: ’Det är ingen mening att ta bilder utfr̊an omr̊adet’, kan förmodligen lösningen snabbas upp avsevärt.

Det är dock inte säkert att diskretiseringen är nödvändig. Diskretiseringen är ett sätt attsätta ut möjliga brytpunkter offline, innan körningen startar. Ett annat sätt att hanteraatt ett hinder stoppar robotens ursprungliga plan är att online beräkna vilka omr̊adensom är i behov av fler bilder och ifr̊an vilka vinklar de behövs och därefter beräkna enmöjlig rutt. Scoutenheten skulle d̊a i princip kunna beräkna nästa brytpunkt och undertiden den rör sig dit. Ta bilder och beräkna fram en möjlig nästnästa brytpunkt. Om etthinder hittas p̊a vägen skulle en ny rutt planeras utifr̊an denna information.


Autonom bandvagn17

LIPs

iMapLiTH

7 oktober 2011

3.4.2 Exempel p̊a algoritmstruktur för diskret modell

Vilket upplägg som än används kommer det att behövas en intern representation avomgivningen och information om vilket tillst̊and roboten befinner sig i. I det diskreta falletkan omgivningen representeras av en lista med alla möjliga brytpunkter. Varje positionbetecknas som oupptäckt, inneh̊aller ett objekt, blockerad eller fri. Detta kan innebäraatt om en brytpunkt är fri finns det inget objekt p̊a den positionen och den behöver intefotograferas, om den är oupptäckt m̊aste den minst fotograferas en g̊ang fr̊an en vinkeloch om den inneh̊aller ett objekt m̊aste den fotograferas fr̊an fyra vinkelräta vinklar. Nären brytpunkt som inneh̊aller ett objekt blir fotograferad fr̊an fyra h̊all räknas den somblockerad och behöver inte fotograferas mer. Genom att vid en given brytpunkt ta en bildi en viss riktning, erh̊alls information om brytpunkten i den riktningen. Denna informationanvänds sedan vid uppdateringen av listan.

De möjliga åtgärderna som finns för roboten kan vara att t.ex. svänga 90 grader åthöger/vänster, ta en bild, åka fram̊at en meter.

Målet för roboten kan vara att alla brytpunkter ska vara antingen fria eller blockerade.

Det som bestämmer vad roboten ska göra härnäst är en sökfunktion som söker igenom demöjliga kommandon som roboten har och värdesätter vilket kommando som leder snabbasttill m̊alet. Det finns ett flertal sökalgoritmer att välja som använder olika strategier. Föratt förenkla kan ”Breadth-first search” användas som väljer det alternativ som är närmast.Om man inte viktar kostnaden av kommandona s̊a att det är bättre att köra rakt fram änatt svänga kan detta dock resultera i att roboten snurrar ett helt varv p̊a varje brytpunkt,vilket kanske inte är det tidsmässigt mest effektiva sättet.

4 Scoutenheten

Scoutenheten best̊ar av en bandvagn av typ MMP30 samt en dator som använder ope-rativsystemet Linux. P̊a bandvagnen sitter tv̊a stycken likströmsmotorer, en p̊a var-dera sida, som driver enheten. Plattformen inneh̊aller även tv̊a batterier som via enspänningsregulator tillhandah̊aller spänning till motorer och övrig elektronik i bandvag-nen. Scoutenheten är utrustad med odometrar p̊a motorerna, en GPS och en stereokamera.Scoutenheten best̊ar av tv̊a stycken delsystem: Navigationsenheten samt framdrivnings-enheten ARM. Kommunikation mellan scoutenheten och omvärlden sker genom naviga-tionsenheten till masterenheten via WLAN.


Autonom bandvagn18

LIPs

4.1 Positionering iMapLiTH

7 oktober 2011

Figur 7: Översikt av scoutenheten.

4.1 Positionering

För att utifr̊an stereobilderna kunna skapa en 3D-värld behövs en bättre positionering äntidigare. Gruppen 8Yare fr̊an 2010 estimerade bandvagnens position genom att behandladata fr̊an Odometer och GPS och med hjälp av ett Extended Kalman Filter (EKF) skattabandvagnens tillst̊and och dynamik. Den estimeringen kommer utökas med att även skattapositionen utifr̊an information fr̊an stereobilderna. Detta görs med Visuell Odometri (VO).Informationen fr̊an de tre sensorerna kommer sedan att fusioneras ihop med hjälp av EKF.

4.1.1 Bibliotek

OpenCv och MRPT är tv̊a programbibliotek som är bra för bildanalys och filtrering. Dessabibliotek inneh̊aller bland annat algoritmerna nedan. De är gratis och g̊ar att ladda nerfr̊an internet och kan inkluderas i C++. Även Rob Hess har användbar kod som finns atthitta gratis p̊a internet [13].

4.1.2 Visuell odometri

VO är en metod för att bestämma position och orientering genom analys av associeradekamerabilder. P̊a det sättet kan ocks̊a bandvagnens rutt bestämmas. Metoden g̊ar ut p̊aatt hitta förändringar i bilden och p̊a s̊a vis kunna identifiera samma omgivning i andrabilder. Genom det kan man sedan bestämma kamerans relativa position när en viss bildtogs gentemot en annan bild.


Autonom bandvagn19

LIPs


7 oktober 2011

Figur 8: Flödesschema över visuell odometri.

4.1.2.1 Hitta punkter

Det första steget i VO är att hitta intressanta punkter, landmärken, i en bild. Landmärkenär pixlar som är väldefinierade och inneh̊aller mycket information om sin position. Detinnebär att de distinkt skiljer sig fr̊an sin omgivning och allts̊a är lätta att identifiera, tillexempel hörn. En metod för att hitta dessa är Harris corner detector [3]. Fördelen meddenna metod är att den är invariant för rotation, skalning, belysning och brus i bilden.Metoden bygger p̊a en lokal auto-korrelationsfunktion som mäter lokala förändringar iintensitet i olika riktningar. Detta genom att placera ut sm̊a ”fönster” (ca 5 × 5 pixlar)och studera deras intensiteter. När ett fönster flyttas runt i bilden kommer intensiteten attförändras. Genom att maximera intensitetsändringen i tv̊a riktningar kan hörn detekteras.Autokorrelationsfunktionen som ska maximeras är

c(u, v) =∑W

[I(ui, vi)− I(ui + ∆u, vi + ∆v)]2 (2)

där W är fönstret med centrum i punkten (u, v) , I intensiteten och ∆u,∆v förflyttningeni u- respektive v-led.

Genom Taylor approximation och utveckling av funktionen erh̊alls Harrisoperatorn

c(u, v) =[∆u ∆v

](∑W

[I2u IuIvIuIv I

2v

])[∆u∆v

]=[∆u ∆v

]C(u, v)

[∆u∆v

]. (3)

Genom att sedan studera egenvärdena λ1, λ2 till matrisen C(u, v) kan tre fall identifieras:

• λ1, λ2 är sm̊a - sm̊a förändringar i omgivningens intensitet och allts̊a inte en intres-sant punkt.


Autonom bandvagn20

LIPs


7 oktober 2011

• λ1 är stort och λ2 är litet eller viceversa - Intensiteten förändras i en riktning vilketbetyder att fönstret visar en kant.

• λ1, λ2 är stora - stora förändringar i omgivningens intensitet i b̊ada riktningar vilketvisar p̊a ett hörn.

Tröskeln som bestämmer om egenvärdet är stort eller litet m̊aste givetvis bestämmas ochkommer att vara betydande för om punkterna är intressanta eller inte.

4.1.2.2 Matcha punkter

När ett antal intressanta punkter har identifierats p̊a tv̊a olika bildpar (höger och vänsterstereobild) s̊a är nästa problem att matcha ihop dessa mellan de olika bildparen. Det finnsett antal olika algoritmer för att göra detta. I detta projekt kommer algoritmen Scale-Invariant Feature Transform (SIFT), [9], att användas eftersom att den är oberoende avrotation, skala och belysning. Dessutom bygger den p̊a Harris corner detecter som användsför att hitta intressanta punkterna. För att p̊a ett bra sätt kunna matcha ihop de intres-santa punkterna s̊a görs fönstret till varje intressant punkt om till en beskrivningsvektorsom kan ses som varje enskild punkts fingeravtryck.

Beskrivningsvektorerna i de ena bildparen jämförs sedan med de andra för att detekteramatchande punkter. Detta görs genom att undersöka hur väl de olika beskrivningsvek-torerna matchar varandra. Eftersom det kommer att finnas brus i bilderna s̊a kommerbeskrivningsvektorerna inte att vara exakt lika. Därför undersöks hur bra den bästa match-ningen är i jämförelse med den näst bästa. Jämförelsen av matchningarna används sedanför att skatta positionen i ett Extended Kalman Filter.

4.1.2.3 Behandling av outliers

Outliers, allts̊a felaktiga matchningar som uppst̊ar i samband med SIFT kommer att be-handlas med den iterativa algoritmen RANSAC (RANdom SAmple Consensus)[4]. RAN-SAC g̊ar ut p̊a att en delmängd av all data väljs ut slumpmässigt och sedan användsför att skatta tillst̊anden med modellen. Dessa tillst̊and testas sedan med all data. Skullepassningen som erh̊alls inte vara tillräckligt bra upprepas algoritmen. En tröskel m̊astebestämmas som avgör hur bra matchning som krävs och algoritmen m̊aste begränsas medett fixt antal iterationer.

4.1.3 GPS

Kommunikationen med GPS-enheten kommer inte förändras n̊agot fr̊an vad tidigare pro-jektgrupper skapat där

Xt =[xt yt

]T+ et.

4.1.4 Odometer

Odometerdatat kommer att användas p̊a samma sätt som tidigare års projektgrupp hargjort, det vill säga estimera hastigheten genom att veta längden p̊a bandvagnens band.Detta kan läsas om i förra årets tekniska dokumentation [8] under avsnitt 4.2.3 och 4.1.2.3


Autonom bandvagn21

LIPs


7 oktober 2011

4.1.5 Fusionering

För att skatta bandvagnens tillst̊and kommer som sagt ett Extended Kalman Filter (EKF)att användas. Data fr̊an odometrar, GPS och visuell odometri kommer tillsammans medrörelsemodellen nedan att användas i filtret. Att inte ett vanligt kalmanfilter användsberor p̊a att vinklarna kommer in olinjärt i dynamiken och därför behövs EKF istället.

Tillst̊andsvektorn som skattas i filtret kommer best̊a av dels positionen p̊a bandvagnen,men även positionen för landmärken i bandvagnens synfält, cirka 10 stycken. Detta gör atttillst̊andsvektorns storlek kommer att variera vid varje tidsuppdatering. Om ett landmärkepredikteras ligga utanför bildramens storlek, tas de bort fr̊an tillst̊andsvektorn.

Tillst̊andsvektorn kommer allts̊a se ut som

X̄ =[XB X

1L ... X

nL

]T,

där XB är bandvagnens globala position och XiL =

[xi yi zi

]Tlandmärkenas globa-

la position. Till att börja med kommer XB =[x y ψ

]Tdär ψ är girvinkeln. Denna

kommer sedan utökas s̊a att även höjden, z, tippvinkeln, θ, och rollvinkeln φ estimeras.

Rörelsemodellen som kommer att användas ser ut som

xt+1 = xt + Tsvt cos(ψ +ωψTs

2 ),

yt+1 = yt + Tsvt sin(ψ +ωψTs

2 ),

zt+1 = zt + Tsvt cos(θ +ωθTs2 ),

ψt+1 = ψt + ωψtTs,θt+1 = θt + ωθtTs,φt+1 = φt + ωφtTs,

ωψt+1 = ωψt ,ωθt+1 = ωθt ,ωφt+1 = ωφt ,

(4)

där hastigheten, vt, och vinkelhastigheten, ωψ, ses som insignaler fr̊an odometrarna.

Tillst̊anden beräknas genom att först göra en tidsuppdatering. D̊a predikteras alla land-märkens och bandvagnens position med hjälp av rörelsemodellen. Denna position översättssedan till pixalkoordinater. Vidare görs en mätuppdatering där de predikterade tillst̊andenjämförs med mätdata och skattas i ett EKF.

4.1.5.1 Fr̊an globala koordinater till pixelkoordinater

För att kunna jämföra landmärkenas predikterade koordinater med de uppmätta, m̊astedessa vara i samma koordinatsystem. De globala koordinaterna måste därför göras om tillpixelkoordinater.

Inledningsvis översätts landmärkena till bandvagnens koordinater. Det görs med ekvatio-nen

Mi = R(ψ)(XiL −XB) (5)

där


Autonom bandvagn22

LIPs


7 oktober 2011

R(ψ) =

cos(ψ) sin(ψ) 0− sin(ψ) cos(ψ) 00 0 1

(6)och Mi det i:te landmärkets koordinater i bandvagnens koordinatsystem.

Dessa 3D koordinater m̊aste sedan kunna tolkas och översättas till 2D i bildernas koordi-natsystem. Detta för att kunna prediktera vilken pixel i bilden landmärket borde befinnasig i. Projiceringen till bildplanet ser ut som

[xkyk

]=

f

Miz

[MixMiy

], (7)

där xk, yk är normaliserade bildkoordinater i kameraplanet. z i det här fallet syftar allts̊ap̊a djupet i bilden. f är kamerans fokallängd som p̊a Bumblebee 2 är 6 mm.

Dessa transformeras till pixelkoordinater genom

[ûv̂

]=

[fx 00 fy

] [xkyk

]+

[hxxkhyyk

](8)

där û, v̂ är pixelkoordinater, fx = sx/f , fy = sy/f och sx, sy är pixelstorleken och harvärdet 7,4 µm. hx och hy är mittpunkten av bildramen.

4.1.5.2 Mätuppdatering

När ovanst̊aende beräkningar har genomförts och tillst̊anden har predikterats, uppdaterasde nya tillst̊anden med mätningarna i ett EKF, κ, enligt

x̂t+1|t+1 = x̂t+1|t + κ

([utvt

]−[ût+1|tv̂t+1|t

]). (9)

För att ett landmärke ska kunna jämföras med motsvarande prediktering i de olika bil-derna, betraktas dess beskrivningsvektorer, se avsnitt 4.1.2.2.

Sammanfattningsvis ser proceduren ut enligt Figur 9

Figur 9: Översiktsbild av fusionering.


Autonom bandvagn23

LIPs

4.2 Kollisionshantering iMapLiTH

7 oktober 2011

De skattade koordinaterna och vinklarna skickas sedan vidare tillsammans med bilder tillkarteringen där de sedan används vid skapandet av en 3D-värld.

4.1.5.3 Dataobjekt

Efter positioneringen kommer sex tillst̊and tillsammans med en bild att kunna skickasvidare till karteringen. Nedan följer en beskrivning p̊a de dataobjekt som positioneringenanvänder sig av.

FeaturesEn intressant punkt best̊aende av koordinater och vilken bild punkten ligger i.

featuresCoordinates Array med punktens koordinater [x,y].

featuresImage Vilken bild punkten ligger i, höger eller vänster.

Key PointsEtt fönster runt den intressanta punken som inneh̊aller koordinater och egenskaper.

keyPointCoordinates Array med mittpunktens koordinater [x,y].

keyPointsFingerPrint Array med fönstrets egenskaper.

Key Point ListHanterar en lista där en bilds alla intressanta fönster finns.

listOfKeyPoints Array med en bilds alla intressanta fönster.

Matching Key PointsTv̊a matchande Key Points, inneh̊aller koordinater och avst̊and.

matchingKeyPointsCoordinates Array med de tv̊a matchande punkternas keypoints

Matching Key Points ListEtt fönster runt den intressanta punken som inneh̊aller koordinater och egenskaper.

listOfMatchingKeyPoints Array med en bilds alla matchande keypoints.

Estimated DataDe skattade koordinaterna och vinklarna.

estimatedCoordinates Array med de skattade koordinaterna [x,y,z]

estimatedAngles Array med de skattade vinklarna [θ, φ, ψ]

4.2 Kollisionshantering

En viktig del att ta hänsyn till under körning av roboten är möjligheten att detekterahinder och kunna välja en lämplig motaktion. Det finns tv̊a olika typer av kollisionshan-tering. En som behandlar detektion och motaktion vid hinder som blockerar färdvägensamt en annan som gäller vid kollision med förbjudna omr̊aden. Förbjudna omr̊aden väljsav användaren i användargränssnittet.

4.2.1 Kollisionshantering för hinder

Om scoutenheten närmar sig ett hinder kommer det att kunna detekteras med hjälp avdjupbilderna. D̊a ett hinder är närmare än en meter bör systemet planera och utföra enmotaktion. De motaktioner som ska implementeras är att planera om rutten enligt detsom anges i avsnitt 3.4 alternativt stanna helt om det inte är möjligt att undvika hindretp̊a annat sätt.


Autonom bandvagn24

LIPs

4.2 Kollisionshantering iMapLiTH

7 oktober 2011

4.2.2 Kollisionshantering för förbjudna omr̊aden

För att detektera att roboten är innanför ett förbjudet omr̊ade används dess skattadeposition. D̊a ruttplaneringen planerar en säker väg ska detta scenario aldrig ske. Omroboten mot all förmodan hamnar innanför skall den köra samma väg ut som den kom ind̊a detta är den enda vägen som med säkerhet är körbar.


Autonom bandvagn25

LIPs

iMapLiTH

7 oktober 2011

A Konventioner

För att f̊a enhetligt utseende p̊a kod och dokument kommer föreg̊aende gruppers konven-tioner användas.

A.1 Dokumentkonventioner

Följande dokumentkonventioner används:

• Inget stycke f̊ar sakna inledande text.

A.2 Kodkonventioner

Följande kodkonventioner används:

• Engelska används vid all namngivning.

• Klasser har formen av substantiv i singularis, t.ex. Odometer eller namnet p̊a mo-dulen t.ex. NetworkModule.

• Metoder har formen Verb alternativt VerbSubstantiv, t.ex. Send, GetAngle.

• Klass- och metodnamn använder PascalCase, vilket innebär att första tecknet ivarje ord är versalt.

• Datamedlemmar, lokala variabler samt parametrar använder camelCase, t.ex. ope-rationList.

• Inga understreck i namn p̊a varken klasser, datamedlemmar eller metoder.

• Indentering är tv̊a positioner, mellanslag (space) inte tab.

• Curly-braces placeras p̊a egen rad med samma indentering som föreg̊aende instruk-tion, sk. Allman indent style.

• Ett mellanrum runt operatorer, dock inte före eller efter parenteser i funktionsde-finitioner och funktionsanrop.

• För integrala typer används direkt anrop med värde, för större typer användskonstantreferenser eller pekare. Samma för returtyper.

• I kodruta 1 följer ett exempel p̊a hur kod i C++ har formateras.

class MyClass : public ABase{

public :int MyMethod( int thisIsAParameter ,

const AnotherClass &anotherClass Input ) ;private :

AnotherClass data ;}

int MyClass : : MyMethod( int thisIsAParameter ,const AnotherClass &anotherClass Input )

{data = anotherClass Input ;


Autonom bandvagn26

LIPs

A.2 Kodkonventioner iMapLiTH

7 oktober 2011

i f ( this IsAParameter

REFERENSER iMapLiTH

7 oktober 2011

Referenser

[1] Herbert Bay, Tinne Tuytelaars, and Luc Van Gool. Surf: Speeded up robust features.In Axel Pinz Ales Leonardis, Horst Bischof, editor, Computer Vision - ECCV 2006,volume 3951 of Lecture Notes in Computer Science, pages 404–417. Springer Berlin/ Heidelberg, 2006. 10.1007/11744023 32.

[2] Boris Delaunay. Sur la sphère vide. a la mémoire de georges voronöı. Bulletin del’Académie des Sciences de l’URSS. Classe des sciences mathématiques et na, 6:793–800, 1934.

[3] Konstantinos G. Derpanis. The harris corner detector. Technical report, Compu-ter Science, York University, October 2004. http://www.cse.yorku.ca/~kosta/CompVis_Notes/harris_detector.pdf.

[4] Konstantinos G. Derpanis. Overview of the ransac algorithm. Technical report,Computer Science, York University, May 2010. http://www.cse.yorku.ca/~kosta/CompVis_Notes/ransac.pdf.

[5] Andreas Geiger. Libelas. C++ library. http://www.rainsoft.de/software/libelas.html.

[6] Andreas Geiger, Martin Roser, and Raquel Urtasun. Efficient large-scale stereomatching. In Asian Conference on Computer Vision (ACCV), Queenstown, NewZealand, November 2010. ttic.uchicago.edu/~rurtasun/publications/geiger_et_al_accv10.pdf.

[7] Andreas Geiger, Julius Ziegler, and Christoph Stiller. Stereoscan: Dense 3d recon-struction in real-time. In IEEE Intelligent Vehicles Symposium, Baden-Baden, Ger-many, June 2011.

[8] Gustav Hanning. Teknisk dokumentation, 2010.

[9] David G. Lowe. Object recognition from local scale-invariant features. In ComputerVision, 1999. The Proceedings of the Seventh IEEE International Conference on, vo-lume 2, pages 1150 –1157 vol.2, 1999. http://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=790410.

[10] Andrew Zisserman Richard Hartley. Multiple View Geometry in Computer Vision.Cambridge University Press, West Nyack, NY, USA, 2 edition, March 2004.

[11] Nader Salman and Mariette Yvinec. Surface Reconstruction from Multi-View Stereo.Lecture notes in computer science, September 2009.

[12] Bill Triggs, Philip McLauchlan, Richard Hartley, and Andrew Fitzgibbon. Bundleadjustment - a modern synthesis. In Bill Triggs, Andrew Zisserman, and Richard Sze-liski, editors, Vision Algorithms: Theory and Practice, volume 1883 of Lecture Notesin Computer Science, pages 153–177. Springer Berlin / Heidelberg, 2000. 10.1007/3-540-44480-7 21.

[13] Rob Hess. Library for SIFT computations. http://blogs.oregonstate.edu/hess/code/sift/


Autonom bandvagn28

LIPs

http://www.cse.yorku.ca/~kosta/CompVis_Notes/harris_detector.pdfhttp://www.cse.yorku.ca/~kosta/CompVis_Notes/harris_detector.pdfhttp://www.cse.yorku.ca/~kosta/CompVis_Notes/ransac.pdfhttp://www.cse.yorku.ca/~kosta/CompVis_Notes/ransac.pdfhttp://www.rainsoft.de/software/libelas.htmlhttp://www.rainsoft.de/software/libelas.htmlttic.uchicago.edu/~rurtasun/publications/geiger_et_al_accv10.pdfttic.uchicago.edu/~rurtasun/publications/geiger_et_al_accv10.pdfhttp://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=790410http://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=790410http://blogs.oregonstate.edu/hess/code/sift/http://blogs.oregonstate.edu/hess/code/sift/

InledningSystemöversiktGrov systembeskrivningIngående delsystem

MasterenhetenKarteringBibliotekSkapa djupbildELAS

Generera punktmolnIhopsättning av punktmolnSkapa polygonerStäda polygonvärldTexturera polygonerDataobjekt från kartering

AnvändargränssnittTidigare funktionalitetVidareutveckling

3D-världenBibliotekKravuppföljningTexturering

RuttplaneringFörenklingarExempel på algoritmstruktur för diskret modell

ScoutenhetenPositioneringBibliotekVisuell odometriHitta punkterMatcha punkterBehandling av outliers

GPSOdometerFusioneringFrån globala koordinater till pixelkoordinaterMätuppdateringDataobjekt

KollisionshanteringKollisionshantering för hinderKollisionshantering för förbjudna områden

KonventionerDokumentkonventionerKodkonventioner

designspeci kation · 2011. 10. 10. · imap lith 7 oktober 2011 designspeci kation emanuel w...

Documents