Positionering och kartuppbyggnad med avståndsmätande laser

Positionering och kartuppbyggnad

med avståndsmätande laser

Examensarbete utfört i Reglerteknik

vid Tekniska Högskolan i Linköping

av

Jonas Kjellander

LiTH-ISY-EX-3388-2003

Linköping 2003

Positionering och kartuppbyggnad

med avståndsmätande laser

Examensarbete utfört i Reglerteknik

vid Tekniska Högskolan i Linköping

av

Jonas Kjellander

LiTH-ISY-EX-3388-2003

Handledare:

Tekn. Dr. Jonas Nygårds, FOI Prof. Åke Wernersson, FOI Lic. Rickard Karlsson, ISY

Examinator:

Division, Department Institutionen för Systemteknik 581 83 LINKÖPING Date 2003-05-16 Språk

Language Rapporttyp Report category ISBN

X Svenska/Swedish

Engelska/English Licentiatavhandling X Examensarbete ISRN LITH-ISY-EX-3388-2003 C-uppsats D-uppsats Serietitel och serienummer _{Title of series, numbering} ISSN

Övrig rapport

____

URL för elektronisk version

http://www.ep.liu.se/exjobb/isy/2003/3388/

Titel

Title Positionering och kartuppbyggnad med avståndsmätande laser Positioning and Mapping using Range Finding Laser

Författare

Author Jonas Kjellander

Sammanfattning

Abstract

This master thesis includes a description of how a number of sensors are put together in a sensor platform. It also gives a description of how localization and mapping has been performed with data collected from the sensor platform.

Localization is a prerequisite for most of the tasks that can be requested from an autonomous mobile robot. In many situations the GPS signal is not available and hence an additional localization system is required. One approach is to apply localization based on landmarks

extracted from the robots surrounding. Using e.g. a scanning range finding laser landmarks can be extracted from the robots surrounding.

Localization and mapping is performed with an extended kalman filter where the robots and the landmarks positions are represented as states. The positioning algorithm is finally tested in indoor environment using lines and line segments as landmarks. The landmarks are extracted from measurement data from the scanning range finding laser. This master thesis is a part of a larger project which purpose is to investigate the benefit of cooperation between several mobile robots.

Nyckelord

Keyword

Sammanfattning

I detta examensarbete beskrivs hur ett antal sensorer sätts samman till en sensorplattform, därefter beskrivs hur positionering och kartuppbyggnad utförs med data insamlad från sensorplattformen.

Positionering är en förutsättning för de flesta uppgifter som rörliga autonoma robotar kan tänkas utföra. I många situationer är GPS signaler inte tillgängliga och ett kompletterande positioneringssystem är nödvändigt. Ett sätt att utföra positionering utan GPS är att relatera robotens position till landmärken i dess omgivning. Med hjälp av t.ex. en svepande avståndsmätande laser kan landmärken extraheras ur mätningar gjorda på omgivningen.

Positioneringen och kartuppbyggnaden utförs genom att robotens och landmärkenas positioner införs som tillstånd i ett extended kalmanfilter. Positioneringsmetoden testas slutligen i inomhusmiljö genom att linjer och linjesegment används som landmärken. Landmärkena är extraherade ur mätdata från en svepande avstånsmätande laser. Detta examensarbete ingår i ett större projekt vars syfte är att undersöka fördelar med flera samverkande robotar.

1.1 Bakgrund till examensarbetet...1 1.2 Principer för positionering ...1 1.3 Syfte...2 1.4 Begränsningar ...2 1.5 Rapportens disposition...2 1.6 Förkortningar ...4 1.7 Beteckningar ...4 2 Sensorplattformen...6 2.1 Farkosten...6

2.2 Beräkning av farkostens förflyttning ...7

2.3 Sensorer ...9

2.3.1 Avståndsmätande laser...9

2.3.2 GPS mottagaren ...11

2.3.3 Vinkelförändringsgivare...11

2.4 Dator och operativsystem...11

2.5 Dataöverföring mellan sensorplattformar ...12

2.6 Sensorplattformens programstruktur...12

2.6.1 Sensorprogram ...13

2.6.2 Datalagringsprogram...14

2.6.3 Nätverksprogram...14

3 Teori för kalmanfilter och associering...16

3.1 Kalmanfilter ...16 3.2 Extended kalmanfilter...16 3.3 Associering ...17 4 Positionering...19 4.1 Simulerad positionering ...19 4.1.1 Systemuppdatering...22 4.1.2 Processbrus...25 4.1.3 Mätmodellen ...27

4.2 Positionering med verklig data...28

4.2.1 Systemuppdatering...30

4.2.2 Processbrus...30

4.2.3 Mätmodellen ...32

4.2.4 Associering av objekt ...35

4.2.5 Lägga till nya objekt...37

5 Extraktion av objekt ur laserdata ...40

5.1 Linjesegmentsobjekt ...40

5.1.1 Extraktion av linjesegment ur laserdata ...40

5.1.2 Behandling av linjesegmentskandidater ...41

5.1.3 Kovariansmatrisen för linjesegmentobjekt...43

5.2 Linjeobjekt ...46

5.2.1 Extraktion av linjeobjekt ur laserdata...48

5.2.2 Houghtransformen...48

5.2.4 Kovariansmatrisen för linjeobjekt ...50

5.2.5 Segmentering av linjeobjekt...52

6 Experiment ...53

6.1 Positionering med linjesegmentsobjekt...53

6.2 Positionering med linjeobjekt ...58

7 Slutsatser och fortsatt arbete...61

7.1 Slutsatser...61

7.2 Förslag till fortsatt arbete ...61

1 Introduktion

Detta examensarbete är utfört åt Institutionen för Data & Sensorfusion som tillhör avdelningen ledningssystem vid Totalförsvarets Forskningsinstitut (FOI) i Linköping. En del av institutionens forskning kring autonoma robotar utförs i samarbete med Institutionen för Konstruktions och Produktionsteknik(IKP) vid Linköpings universitet.

1.1 Bakgrund till examensarbetet

Detta examensarbete är en del av ett större projekt som undersöker egenskaper och möjliga tillämpningar för autonoma mobila robotar i militära applikationer. Intressanta tillämpningar är

Semiautonoma förelöpare till ett större fordon. Förelöparna ska vara offringsbara och förväntas utföra exempelvis rekognosering.

Självständiga övervakningsrobotar som arbetar i riskabla miljöer. Oavsätt tillämpning behöver en autonom robot en robust positioneringsmetod. Det finns flera olika metoder för att lösa denna typ av problem, t.ex. kalmanfilter och partikelfilter, [1], [2]. Några faktorer är gemensamma för alla metoder, framförallt valet av sensorer och det faktum att möjligheten till samarbete mellan flera robotar ger stora fördelar. Den vanligaste sensorkombinationen är

vinkelförändringsgivare monterade på hjulen, avståndsmätande sensorer och GPS-mottagare. Det här examensarbetet är framförallt inriktat på att utföra positionering då GPS signaler inte är tillgängliga. Tillfällen då GPS signaler inte är tillgängliga förekommer relativt ofta t.ex. i närheten av stora byggnader eller i inomhusmiljö.

Som utgångspunkt för positioneringsalgoritmen används ett extended kalmanfilter, vilket används i forskningsarbete vid IKP för att simulera

positionering med flera robotar och objekt [3]. Examensarbetet är utfört i nära samarbete med ett annat examensarbete [4]. Samarbetet har till största delen rört positioneringsalgoritmer och positioneringsexperiment med en robot. Ett område som endast beskrivs i detta examensarbete är uppbyggnaden av de program som används för att kommunicera med sensorerna. Områden som endast berörs i [4] är upptäckt av förändring och positioneringsexperiment med två robotar.

1.2 Principer för positionering

Robust positionering är en grundförutsättning för de flesta uppgifter som rörliga autonoma robotar kan tänkas utföra. Ett enkelt sätt att utföra positionering av en robot är att beräkna robotens position utgående från hjulens vinkelförändring, så kallad dödräkning. Att på detta sätt beräkna robotens position innebär att

eventuella fel ackumuleras vilket i sin tur gör att osäkerheten om robotens position ökar desto längre sträcka den färdas. För att undvika att osäkerheten om robotens position ökar måste robotens position kunna relateras till omgivningen.

Introduktion

. .

Sensorer som är fästa på roboten och gör mätningar på robotens omgivning innebär att robotens position kan relateras till omgivningen. Från sensorerna kan roboten extrahera mätningar på landmärken. Genom att associera mätningarna med kända landmärken i en karta kan en uppdatering av robotens position göras. I vissa tillämpningar kan förhandsinformation om omgivningen vara tillgänglig, i andra kan förhandsinformation helt saknas. I fall då förhandsinformation helt saknas måste roboten samtidigt som den bygger en karta uppdatera sin position med densamma. Detta problem brukar kallas simultaneous localization and mapping (SLAM). Utgående från en startposition extraherar roboten landmärken från mätningar på omgivningen och sparar deras position. Så länge alla

landmärken är synliga kan roboten i teorin bestämma sin position med samma säkerhet som första gången den gjorde mätningar på landmärkena. Genom att lägga till nya landmärken i kartan samtidigt som den gör mätningar på gamla landmärken kan roboten utvidga sin karta med fortsatt god uppfattning om den egna positionen.

1.3 Syfte

Syftet med examensarbetet är att sätta samman ett antal sensorer till en fungerande sensorplattform, vilken ska vara anpassad för att monteras på bandvagnen som visas i figur 2-1. Data från sensorerna ska via ett trådlöst nätverk kunna hämtas av andra liknande sensorplattformar eller av andra datorer som är anslutna till samma nätverk som sensorplattformen. Möjlighet skall även finnas att spara data från sensorerna i en fil.

Då sensorplattformen fungerar ska existerande algoritmer för simulering av positionering och kartuppbyggnad modifieras så att positionering och

kartuppbyggnad kan utföras utgående från verklig data. Dessa algoritmer ska sedan testas med data insamlad från sensorplattformen.

1.4 Begränsningar

Vid positioneringen har vissa förenklingar gjorts. En förenkling är att objekten och roboten antas befinna sig i samma plan dvs. positioneringsalgoritmen arbetar i två dimensioner. En annan förenkling är att positioneringen utförts på

insamlade mätningar och inte i realtid. Ytterligare en förenkling är att antalet objekt som används vid positionering antas vara så få att beräkningstiden inte blir för stor.

1.5 Rapportens disposition

Arbetet och resultaten i denna rapport är presenterade på följande sätt:

Kapitel 1 I detta kapitel beskrivs bakgrunden till examensarbetet följt av en introduktion till positionering och kartuppbyggnad.

Kapitel 2 Beskrivning av sensorplattformen och de dataprogram som används för att kommunicera med sensorerna.

Kapitel 3 Introduktion till teorin för kalmanfilter och associering. Detta kapitel kan hoppas över av den läsare som är bekant med teorin för kalmanfilter och extended kalmanfilter.

Kapitel 4 I detta kapitel beskrivs först de algoritmer för simulering och kartuppbyggnad med flera samverkande robotar som används som utgångspunkt för positionering och kartuppbyggnad. Sedan följer en beskrivning av de ändringar och tillägg som utförts för att verkliga mätningar från en avståndsmätande laser ska kunna införas i algoritmerna.

Kapitel 5 I detta kapitel beskrivs hur två typer av landmärken extraherats från mätningar utförda av den svepande avståndsmätande lasern. De landmärken som extraherats är linjesegmentsobjekt och linjeobjekt.

Kapitel 6 Beskrivning av resultat från test av algoritmerna för positionering och kartuppbyggnad utförd i inomhusmiljö.

Kapitel 7 Förslag till fortsatt arbete samt slutsatser dragna från bakgrunden till examensarbetet och resultaten från experimenten utförda i kapitel 6.

Introduktion

. .

1.6 Förkortningar

EKF Extended kalmanfilter.

FOI Totalförsvarets forskningsinstitut. GPS Global positioning system.

IKP Institutionen för konstruktions och produktionsteknik. IMU Inertial measurement unit.

LiTH Linköpings tekniska högskola. NNSF Nearest neighbor standard filter. SLAM Simultaneous localization and mapping. W-LAN Wireless local area network.

1.7 Beteckningar

a Hough parameter som beskriver stegvidden.

C Houghtransformens värden.

d2 Mahalanobisavståndet.

D Matris innehållande mahalanobisavstånd. δs Förflyttad sträcka för farkostens axelcentrum.

δx, δy Relativ förflyttning av farkostens x respektive y position. δϕ Vinkelförändring av farkostens position.

∆d Hough avståndssteg. ∆ϕ Hough vinkelsteg.

eφ Funktion som beräknar farkostens vridning med hjälp av

hjulens vinkelförändring och farkostens geometri.

es Funktion som beräknar farkostens förflyttning med hjälp av

hjulens vinkelförändring och farkostens geometri.

f Systemfunktion för EKF.

F Systemmatris för det linjära kalmanfiltret

g Funktion som transformerar en mätning från ett robotrelativt koordinatsystem till det jordfasta koordinatsystemet.

gh Viktfunktion för Houghtransformen.

G Matris som påverkar styrsignalerna till det linjära kalmanfiltret.

Gx Jacobian av g med avseende på x för EKF.

Gz Jacobian av g med avseende på z för EKF.

γ Hough vinkel.

h Mätfunktion för EKF.

H Mätmatris för det linjära kalmanfiltret och jacobianen av h

med avseende på x för EKF.

I Identitetsmatrisen.

k Tidsindex i EKF.

K Kalmanförstärkningen.

Ld Det kortaste avståndet mellan farkostens nuvarande

axelcentrum och axelcentrum vid föregående tidpunkt.

Lrh Radie från farkostens rotationscentrum till höger hjul.

Lrv Radie från farkostens rotationscentrum till vänster hjul.

Lh Längd i Houghtransformen.

M Rotationsmatris.

η Avståndet mellan en mätpunkt och punkten som representerar ett objekt.

P Kovariansmatrisen för skattningsfelet i EKF.

Q Processbrusets kovariansmatris.

R Mätbrusets kovariansmatris.

ρ Ortogonala avståndet till en Houghlinje.

sh Förflyttad distans för höger hjul.

sv Förflyttad distans för vänster hjul.

S Kovariansen för innovationen i EKF.

σϕ Standardavvikelse för mätningar av riktningen på linjer och linjesegment.

σl Standardavvikelse för mätningar på linjer och linjesegment i

deras längdriktning.

σr Standardavvikelse för mätningar på linjer och linjesegment i

deras normalriktning.

u Insignaler till kalmanfiltret.

ua Avståndet mellan höger och vänster hjul.

uθh Vinkelförändring för höger hjul.

uθv Vinkelförändring för vänster hjul.

urv Vänster hjulradie.

urh Höger hjulradie.

us Styrsignal till robot, önskad förflyttning.

uφ Styrsignal till robot, önskad vridning.

v Mätbrus.

V Jacobian av h med avseende på v.

w Systembrus för kalmanfilter och EKF.

wh Fönsterfunktion för houghtransformen.

W Jacobian av f med avseende på systembruset w. x, xˆ Tillståndsvektorn och estimerat tillstånd i process och

mätmodellen.

Sensorplattformen

. .

2 Sensorplattformen

Då examensarbetet påbörjades var intentionen att en sensorplattform skulle sättas samman genom att bandvagnen i figur 2-1 utrustades med ett antal sensorer. Denna bandvagn saknar dock ett fungerande styrsystem. Ett annat examensarbete som utförs vid IKP syftar dock till att utveckla ett sådant [5].

Figur 2-1: Bandvagnen som ska bära sensorplattformen.

Eftersom bandvagnen inte kunde användas som sensorbärare brukades istället en vagn till detta. Denna vagn utrustad med ett antal sensorer kommer i

fortsättningen hänvisas till som farkosten.

2.1 Farkosten

Farkosten består av en fyrhjulig vagn utrustad med en dator och ett antal sensorer. De sensorer som farkosten är utrustad med är en svepande avståndsmätande laser, två vinkelmätande sensorer monterade på vardera bakhjulet och en GPS-mottagare. Den avståndsmätande svepande lasern är placerad på vagnen så att avståndsmätningarna utgår från en position rakt ovanför bakaxelns centrum. Farkosten styrs med hjälp av de båda framhjulen medan bakhjulen är passiva, oberoende och rullar fritt. Farkosten är två meter lång, en meter bred och har luftfyllda gummihjul, som ger bra friktion vid låga hastigheter. Farkosten saknar egen drivning och måste förflyttas för hand eller dras av ett annat fordon. Ett foto av vagnen utrustad med den avståndsmätande lasern och de vinkelförändringsmätande sensorerna visas i figur 2-2.

Figur 2-2: Vagnen på bilden har utrustats med vinkelmätande sensorer på vardera bakhjulet och en avståndsmätande svepande laser är placerad mitt ovanför centrum av bakaxeln.

2.2 Beräkning av farkostens förflyttning

I detta avsnitt beräknas den förflyttning farkosten utfört mellan två avläsningar av vinkelförändringsgivarna. Den förflyttning som söks är den sträcka som axelcentrum, där lasern är placerad, har färdats samt vinkelförändringen av bakaxeln. Den sökta sträckan benämns δφ och den sökta vinkelförändringen benämns δφ. Då farkostens förflyttning beräknas görs antagandet att farkostens rörelse, under ett samplingsintervall, går att beskriva som en rörelse längs en cirkelbåge [6]. De variabler som används vid beräkning av farkostens

förflyttning är vinkelförändringen för höger bakhjul under ett sampelintervall (uθh), vinkelförändringen för vänster bakhjul under ett sampelintervall (uθv), radien för höger bakhjul (urh), radien för vänster bakhjul (urv) och avståndet

mellan de båda bakhjulen (ua). I figur 2-3 visas förflyttningen som farkosten

utfört under ett samplingsintervall genom att farkostens bakhjul och bakaxel visas vid tidpunkterna k-1 och k.

Sensorplattformen

. .

Figur 2-3 : I figuren visas bakaxeln till farkosten vid två på varandra efterföljande tidpunkter.

Nedan definieras de variabler som används i figur 2-3

δφ Ändring av vagnens riktning.

δs Förflyttad distans för axelcentrum.

sh Förflyttad distans för höger hjul

sv Förflyttad distans för vänster hjul.

urh Höger hjulradie.

urv Vänster hjulradie.

ua Avståndet mellan höger och vänster hjul.

uθh Vinkelförändring för höger hjul.

uθv Vinkelförändring för vänster hjul.

Lrc Radie från rotationscentrum till axelcentrum.

Lrh Radie från rotationscentrum till höger hjul.

Lrv Radie från rotationscentrum till vänster hjul.

es Funktion som beräknar farkostens förflyttning med hjälp av

hjulens vinkelförändring och farkostens geometri.

eφ Funktion som beräknar farkostens vinkelförändring med hjälp

av hjulens vinkelförändring och farkostens geometri.

Tabell 2-1: Variabler som används för att beräkna farkostens relativa förflyttning.

Nedan beräknas δφ och den sträcka δs som farkosten färdats under ett

sampelintervall dvs. mellan tidpunkterna k och k-1. Utgående från farkostens geometri och vinkelförändringen för de båda bakhjulen på farkosten beräknas funktionen

      =       ) , , , , ( ) , , , , ( a rv rh v h a rv rh v h s u u u u u e u u u u u e s θ θ ϕ θ θ δϕ δ . ( 2-1)

Några grundläggande samband ges av ekvationerna nedan. δϕ θv rv rv v u u L s = = ( 2-2) δϕ θh rh rh h u u L s = = ( 2-3) δϕ δs =Lrc ( 2-4) a rh rv L u L − = ( 2-5) 2 rv rh rc L L L = + . ( 2-6)

Ekvation (2-2), (2-3) och (2-5) ger

(

rv v rh h

)

a u u u u u θ θ δϕ = 1 − , ( 2-7)

och (2-2)- (2-5) och (2-7) ger

(

Lrv Lrh

)

(

urvu v urhu h

)

s δϕ δϕ _{θ θ} δ = + = + 2 1 2 1 . ( 2-8)

Funktionen som beräknar farkostens vinkelförändring och den sträcka som farkosten färdats blir följaktligen

(

)

(

)

            − + =       =       h rh v rv a h rh v rv a rv rh v h a rv rh v h s u u u u u u u u u u u u u u e u u u u u e s θ θ θ θ θ θ ϕ θ θ δϕ δ 1 2 1 ) , , , , ( ) , , , , ( . ( 2-9)

2.3 Sensorer

De sensorer som är inkopplade till sensorplattformen är en svepande avståndsmätande laser, en GPS-mottagare och två sensorer som mäter vinkelförändring.

2.3.1 Avståndsmätande laser

Den svepande avståndsmätande lasern kommer från tillverkaren SICK [7]. Den kan maximalt svepa över en sektor på 180 grader och göra två avståndsmätningar per grad. Lasern kan även göra fyra mätningar per grad, den sektor som avsöks är då 100 grader. Om lasern är ansluten till en RS422 serieport, som har en

överföringshastighet på 500 kbaud, uppdateras varje mätpunkt i ett lasersvep var 13 ms om en avståndsmätning görs per grad, var 26 ms om två

avståndsmätningar görs per grad och var 53 ms om fyra avståndsmätningar görs per grad. Lasern är dock ansluten till datorn på sensorplattformen via en RS232 serieport som har en maximal överföringshastighet på 38.4 kbaud. Denna överföringshastighet gör att lasersvep kan överföras till datorn med ca 3 Hz då lasern sveper 180 grader och tar två mätningar per grad.

Sensorplattformen

. .

Figur 2-4: Typiskt lasersvep från inomhusmiljö.

Figur 2-5: Sensorplattformen i den position där lasersvepet i figur2-4 är taget.

I figur 2-4 visas ett lasersvep från inomhusmiljö. Lasern befinner sig i origo, den är inställd att svepa 180 grader och göra två avståndsmätningar per grad.

Sensorplattformen befinner sig i ett stort rum och lasern är riktad mot en dörröppning. Ett foto av sensorplattformen då den befinner sig på den plats där lasersvepet i figur 2-4 är taget visas i figur 2-5.

2.3.2 GPS mottagaren

GPS-mottagaren är av märket Allstar från tillverkaren Bae Systems Canada inc. Den levererar data med en hastighet av 1 Hz. Denna sensor har anslutits till sensorplattformen men data från den används ej i detta examensarbete.

2.3.3 Vinkelförändringsgivare

Då experiment utförs monteras sensorplattformen på en vagn utrustad med sensorer som mäter bakhjulens vinkelförändring. Sensorerna är från tillverkaren FAULHABER och har tre kanaler vardera. Figur 2-6 visar de tre kanalerna från en av sensorerna. På kanalerna A och B levererar sensorn 500 pulser per varv och på kanal I levereras en puls per varv. Kanal A och B är förskjutna en halv pulslängd i förhållande till varandra. Om pulsen på kanal A kommer före pulsen på kanal B så vrids sensorn medurs vilket är fallet i figur 2-6. Genom att räkna både positiv och negativ flank på både kanal A och kanal B fås en upplösning som är 2000 steg per varv.

Figur 2-6: A , B och I är utsignaler från en sensor. Tiden mellan två positiva flanker på A eller B motsvarar den tid det tagit för sensorn att vridas en femhundradels varv. Om A blir hög då B fortfarande är låg vrids sensorn medurs. Signalen I blir hög en gång per varv.

2.4 Dator och operativsystem

Den enhet som sköter kommunikationen med sensorerna är en PC-104 med QNX som operativsystem. För att tillåta dataöverföring mellan flera samverkande sensorplattformar är även ett trådlöst nätverk anslutet till datorn. Det trådlösa nätverket är ett W-LAN från tillverkaren D-Link. En PC-104 är i princip en vanlig PC som anpassats för inbyggda och mobila system. Den är liten,

strömsnål, stöttålig och drivs med batterier. QNX är ett realtidsoperativsystem som till sin uppbyggnad är ganska likt operativsystemen UNIX och LINUX. Enligt [8] finns det i QNX ett speciellt stöd för att skriva drivrutiner för externa

Sensorplattformen

. .

enheter, t ex sensorer. Drivrutiner som skrivs med hjälp av detta stöd kallas resurshanterare. En resurshanterare är ett program som skapar ett gränssnitt mot sökvägsrymden genom operativsystemets standard I/O-funktioner. Till

operativsystemet QNX finns ett program som heter Phindos. Detta program ger möjlighet att logga in på datorn via ett nätverk, vilket är smidigt eftersom man slipper att ha skärm och tangentbord kopplat till datorn på sensorplattformen då experiment utförs.

2.5 Dataöverföring mellan sensorplattformar

Samverkan mellan flera robotar förutsätter att dessa har tillgång till data från varandras sensorer. För att uppnå detta måste de kommunicera med varandra på något sätt. I detta examensarbete förutsätts att robotarna kommunicerar via ett trådlöst nätverk. För att göra data från en sensor tillgänglig för andra datorer som är anslutna till nätverket används fyra typer av datorprogram. Varje sensor ansluten till sensorplattformen har ett sensorprogram, ett datalagringsprogram och ett nätverksprogram. Sensorprogrammet sköter all kommunikation med sensorn. Detta program anropas i sin tur av datalagringsprogrammet som lagrar information från sensorn på en fil, eller av nätverksprogrammet som skickar information från sensorn vidare till en annan dator. Ett scenario där två sensorer är inkopplade till sensorplattformen visas i figur 2-7.

Figur 2-7: Exempel på hur programstrukturen och dataflödet kan se ut då två sensorer är inkopplade till sensorplattformen och två externa datorer hämtar data från sensor 1 via nätverket.

2.6 Sensorplattformens programstruktur

Till varje sensor ansluten till sensorplattformen behövs tre typer av program för att göra data från sensorn tillgänglig för andra datorer via nätverket. Ett scenario

där två sensorer är inkopplade till sensorplattformen och två externa datorer hämtar data från en sensor vi nätverket visas i figur 2-7. Som en del i detta examensarbete har sensorprogram, datalagringsprogram och nätverksprogram skrivits för den svepande avståndsmätande lasern och för GPS-mottagaren. Samtliga dataprogram som används av sensorplattformen är skrivna i

programmeringsspråket C++. Eftersom vagnen är en temporär lösning, i väntan på ett fungerande styrsystem till bandvagnen i figur 2-1, har inga program skrivits för vinkelförändringsgivarna. Då experiment utförts med vagnen har programmen till lasern modifierats så att även data från vinkelförändringsgivarna lagrats med dessa.

2.6.1 Sensorprogram

För varje sensor finns ett sensorprogram som sköter all kommunikation med sensorn. Sensorprogrammet anropas av andra program som handhar information från sensorn. Sensorprogrammen är skrivna som resurshanterare. En

resurshanterare är ett program som skapar ett gränssnitt mot sökvägsrymden genom operativsystemets I/O-funktioner som används för att kommunicera med t.ex. serieporten, diskettstationen, etc. Då ett sensorprogram skrivits har en tom resurshanterare använts som utgångspunkt. En tom resurshanterare kan anropas med alla standard I/O-funktioner i operativsystemet men ingen av funktionerna utför något. Den tomma resurshanterare som använts som utgångspunkt då sensorprogrammen skrivits är hämtad från [8]. Då en resurshanterare startas hamnar dess namn i den allmänna sökvägsrymden och den kan anropas av operativsystemets standard I/O-funktioner. De av operativsystemets standard I/O-funktioner som är överlagrade så att något utförs när de anropas är:

Open

Då funktionen Open anropas skickas meddelade till sensorn om att data ska börja sändas. Därefter startas en tråd som med ett visst tidsintervall hämtar data från serieporten. Denna tråd är skyddad av ett lås som gör att det inte startas flera trådar om det är flera klienter som begär data från resurshanteraren samtidigt. Då tråden upptäcker att ett nytt datapaket börjar överföras från sensorn avläses datorns interna klocka och tiden vid avläsningen adderas till datapaketet från sensorn. Valet av hur stort tidssteget ska vara mellan varje avläsning av serieporten avgörs av två saker. Dels hur noga tidsmärkningen ska vara, dels hur mycket resurshanteraren får belasta processorn. För en noggrann tidsmärkning av datapaketet krävs täta avläsningar av serieporten, men att hämta många små datamängder från en serieport är mer resurskrävande än att hämta några stora datamängder. Då ett nytt datapaket är helt överfört från sensorn meddelar tråden funktionerna IOnotify och Select att ett datapaket levererats från sensorn.

Close

Då funktionen Close anropas av en klient avslutas tråden som läser av serieporten, förutsatt att ingen annan klient också har öppnat

Sensorplattformen

. .

Read

Då funktionen Read anropas av en klient returneras det antal bytes klienten har begärt att få. Om antalet byte som klienten läser är färre än det antal byte som datapaketet innehåller får klienten följaktligen inte hela datapaketet. Om antalet byte som klienten läser är fler än det antal byte datapaketet innehåller returneras meddelandet fil slut i de

överflödiga bytena. IOnotify

Med funktionen IOnotify kan en klient begära att bli meddelad då ett nytt datapaket har överförts från sensorn och är klart för att hämtas med funktionen read.

Select

Funktionen Select fungerar på samma sätt som IOnotify. Funktionerna som används för att kommunicera med den svepande

avståndsmätande lasern från tillverkaren SICK bygger på koden till robotservern Player [9]. Player är skriven för operativsystem LINUX och hanterar ett antal olika sensorer. Funktionerna som sköter kommunikationen med lasern har lyfts ut ur programmet och anpassats för operativsystemet QNX. Funktioner för kommunikation med GPS-mottagaren erhölls från Sven Rönnbäck som är doktorand vid Luleå Universitet även dessa funktioner var skrivna för operativsystemet LINUX och fick följaktligen anpassas till operativsystemet QNX.

2.6.2 Datalagringsprogram

För varje sensorprogram finns ett datainsamlingsprogram som lagrar data från sensorn på en fil. Då datalagringsprogrammet startas anges hur många datapaket som skall lagras på filen. Om lagringen önskas avbrytas innan det antal

mätningar som angavs då programmet startades har lagrats görs detta genom ett tryck på någon av bokstavstangenterna följt av inmatningstangenten. När

datalagringsprogrammet startas öppnas sensorprogrammet med funktionen Open. Funktionen Select används av datalagringsprogrammet för att få meddelande om när det finns ett nytt datapaket att hämta i sensorprogrammet. Datapaketet hämtas från sensorprogrammet med funktionen read. Då ett datapaket är hämtat från sensorprogrammet lagras det på filen. Då önskat antal datapaket lagrats på filen stängs sensorprogrammet med funktionen Close och sedan avslutas

datalagringsprogrammet.

2.6.3 Nätverksprogram

Till varje resurshanterare finns ett nätverksprogram som gör data från sensorn tillgänglig på ett nätverk. Då nätverksprogrammet startas anges numret på den port som data ska överföras via. För att ta emot data på den angivna porten behövs ytterligare ett nätverksprogram. När data hämtats från sensorplattformen via det trådlösa nätverket används en toolbox till MATLAB [10]. Då två

startas. I nätverksprogrammet lagras de senaste datapaketen hämtade från sensorprogrammet. Antalet datapaket som lagras i ett nätverksprogram har valts till 100. Då 100 datapaket finns lagrade i nätverksprogrammet och ytterligare ett datapaket hämtats ersätts det äldsta datapaketet med det nya. Den dator som hämtar data från nätverksprogrammet kan begära data på ett antal olika sätt. Nedan följer en beskrivning av de olika sätt en nätverksdator kan hämta datapaket från nätverksprogrammet på:

Senaste datapaketen. Då en nätverksdator begär de senaste

datapaketen skickas det antal datapaket som nätverksdatorn begärt. Det äldsta skickas först och det senaste datapaket som senast skickas sist. Om nätverksdatorn begär fler datapaket än vad som finns lagrade i nätverksprogrammets skickas datapaketen som finns lagrade.

Nästa inkommande datapaket. Om en nätverksdator begär att få nästa inkommande datapaket väntar nätverksprogrammet tills ett nytt

datapaket hämtats från sensorprogrammet. Detta datapaket skickas sedan till nätverksdatorn.

De datapaket som inkommit sedan det senaste datapaketet

skickades. När en nätverksdator gör denna begäran skickas de

datapaket som hämtats från sensorprogrammet sedan det senaste datapaketet skickades. Om det har inkommit fler datapaket än vad som finns lagrade i nätverksprogrammet skickas de som finns lagrade. Då ett nätverksprogram startas måste tillhörande sensorprogram redan vara startat. Det första som görs är att en Socket-server sätts upp på den angivna porten i nätverket. Koden för att sätta upp en Socket-server är hämtad ur ett program skrivet i ett teknologprojekt benämnt Marwin som utförts vid IKP. Då en Socket-server satts upp öppnas sensorprogrammet med funktionen Open. Därefter startas en tråd som hämtar data från sensorprogrammet. Tråden använder funktionen Select för att bli meddelad att sensorprogrammet mottagit ett nytt datapaket från sensorn. Då tråden meddelats att sensorprogrammet mottagit ett nytt datapaket hämtas detta med funktionen read. Det hämtade datapaketet lagras i nätverksprogrammet. Om det redan finns 100 datapaket lagrade ersätts det äldsta datapaketet.

Teori för kalmanfilter och associering

. .

3 Teori för kalmanfilter och associering

I detta kapitel ges en introduktion till teorin för kalmanfilter och extended kalmanfilter. Kalmanfiltret introducerades av R. E. Kalman år 1960 [11] och har sedan dess använts i många olika typer av applikationer. Den beskrivning av kalmanfilter och EKF som ges i detta kapitel är en sammanställning av en beskrivning som görs i [12], i den beskrivningen refereras till [11], [13], [14], [15], [16], [17] och [18]. Ett normalt kalmanfilter förutsätter att modellen som beskriver systemet är linjär. Många verkliga system är dock inte linjära. En metod att hantera olinjära system är extended kalmanfilter (EKF).

3.1 Kalmanfilter

Under vissa förutsättningar ger ett kalmanfilter den bästa möjliga skattningen av tillstånden x givet ett antal mätningar z. Förutsättningarna är att modellen är linjär och att processbruset är vitt och gaussiskt. En linjär modell där xk

representerar de sanna tillstånden vid tidssteg k och uk representerar insignalerna

kan skrivas som

1 1 1 − − − + + = _{k k k} k Fx Gu w x ( 3-1) k k k Hx v z = + . ( 3-2)

Där matrisen G anger hur insignalerna påverkar tillstånden. H är mätmatrisen och

F är systemmatrisen. Processbruset som betecknasw_k ~ N(0,Q_k) och mätbruset som betecknas vk ~ N(0,Rk)antas vara oberoende av varandra. För en sådan modell ges tidsuppdateringen i kalmanfiltret av

1 1 1 1 ˆ ˆ_kk− = Fx_{k− k−} +Gu_k− x ( 3-3) 1 1 1 1 − − − − = k k T + k k k FP F Q P . ( 3-4)

Där P är kovariansmatrisen för skattningsfelet. Mätuppdateringen i filtret ges av

k T k k k HP H R S = | ₋₁ + ( 3-5) 1 1 | − − = kk T k k P H S K ( 3-6) ] ˆ [ ˆ ˆ_k|k =x_k|k−1+K_k z_k −Hx_k|k−1 x ( 3-7) 1 | |k =[ − k ] kk− k I K H P P . ( 3-8)

I ekvationerna ovan representerar Sk kovariansen för innovationen, I

enhetsmatrisen och Kk kalmanförstärkningen.

3.2 Extended kalmanfilter

Många verkliga system är olinjära. Extended kalmanfilter är ett sätt att hantera olinjära system, dock är det en suboptimal metod. I ett EKF linjariseras modellen kring det aktuella estimatet. Det finns inga garantier för att ett EKF fungerar utan det som avgör hur bra ett EKF fungerar är hur väl systemet approximeras av

linjariseringen. En olinjär modell där xk representerar de sanna tillstånden vid

tidssteg k och uk representerar insignalerna kan skrivas som

) , , ( −1 −1 −1 = k k k k f x u w x ( 3-9) ) , ( k k k h x v z = . ( 3-10)

Tidsuppdateringen för ett EKF ges av ) 0 , , ˆ ( ˆk_|k₋₁ = f xk_−1|k₋₁ uk₋₁ x ( 3-11) T k k k T k k k k k k F P F W Q W P_{| −1}= _{−1| −1} + ₋₁ . ( 3-12) Där 0 , , ˆ _{−1 −1} ∂ ∂ = k k k u x k k w f W ( 3-13) och 0 , , ˆ _{−1 −1} ∂ ∂ = k k k u x k k x f F . ( 3-14)

Mätuppdateringen för ett EKF ges av

T k k k T k k k k k H P H V R V S = | ₋₁ + ( 3-15) 1 1 | − − = T _k k k k k P H S K ( 3-16) )] 0 , ˆ ( [ ˆ ˆk_|k =xk_|k₋₁+ Kk zk −h xk_|k₋₁ x ( 3-17) 1 | 1 | |k = kk− − k k kk− k P K H P P . ( 3-18) Där 0 , ˆ ₋₁ ∂ ∂ = k k x k k x h H ( 3-19) och 0 , ˆ ₋₁ ∂ ∂ = k k x k k v h V . ( 3-20)

3.3 Associering

I detta avsnitt beskrivs en lösning till associeringsproblemet. En generell

beskrivning av associeringsproblemet är att para ihop ett antal mätningar med ett antal objekt. I det generella fallet kan det finnas falska mätningar, flera mätningar på samma objekt, mätningar på okända objekt och objekt som ej blivit inmätta. Ett antal olika sätt att hantera associationsproblemet finns beskrivet i litteratur som handlar om estimering och målföljning. En av de enklaste metoderna att hantera associeringsproblemet är enligt [19] the nearest-neighbor standard filter (NNSF) [20]. I ett NNSF används den viktade normen för innovationen som avståndsmått.

Teori för kalmanfilter och associering

. .

Detta avstånd kallas även mahalanobisavståndet och ges av

[

z zk k

]

T S k

[

z zk k

]

z

d2( )= − ˆ +1| −1 +1 − ˆ +1| . ( 3-21) I ekvationen ovan representerar S kovariansmatrisen för innovationen i

mätningen. Om det finns n objekt som kan ha genererat en mätning och m mätningar har erhållits ges alla möjliga mahalanobisavstånd av elementen i

          = ) ˆ , ( ) ˆ , ( ) ˆ , ( ) ˆ , ( ) , ( 2 1 2 1 2 1 1 2 n m m n z z d z z d z z d z z d n m D " # % # " . ( 3-22)

När mahalanobisavståndet beräknats för alla objekt och alla mätningar ska de objekt och de mätningar som minimerar det totala mahalanobisavståndet

associeras. För att minimera det totala mahalanobisavståndet jämförs summan av alla möjliga kombinationer av objekt och mätningar. Att minimera det totala mahalanobisavståndet är ett beräkningskrävande optimeringsproblem. För att minska komplexiteten och beräkningstiden bör de orimliga värdena i (3-22) sorteras bort. Detta kan göras på statistisk väg med hjälp ett tröskelvärde för mahalanobisavståndet. Utifall inga falska mätningar eller inga mätningar på okända objekt erhålls är mahalanobisavståndet chi-kvadrat fördelat. Genom att studera en tabell för chi-kvadratfördelningar ges en uppfattning om vilken storleksordning tröskelvärdet för mahalanobisavståndet ska vara.

4 Positionering

Robust positionering är en grundförutsättning för de flesta uppgifter som rörliga autonoma robotar kan tänkas utföra. Ett enkelt sätt att utföra positionering av en robot är att beräkna robotens position utgående från hjulens vinkelförändring, så kallad dödräkning. Att på detta sätt beräkna robotens position innebär att

eventuella fel ackumuleras vilket i sin tur gör att osäkerheten om robotens position ökar desto längre den färdas. För att undvika att osäkerheten om

robotens position ökar måste robotens position kunna relateras till omgivningen. Sensorer som är fästa på roboten och gör mätningar på robotens omgivning gör att robotens position kan relateras till omgivningen. Från sensorerna kan roboten extrahera mätningar på landmärken. Den metod som används i detta

examensarbete för att relatera robotens position till omgivningen, är att robotens och landmärkenas positioner införs som tillstånd i ett extended kalmanfilter. Den sensor som används för att extrahera landmärken ur robotens omgivning är den svepande avståndsmätande lasern. De extraherade objekten representeras av en punkt och en riktning. Då positioneringsalgoritmerna tagits fram har algoritmer för simulering av positionering och kartuppbyggnad med flera samverkande robotar använts som utgångspunkt. Dessa algoritmer används vid IKP i forskning kring fördelar med flera samverkande mobila robotar [3].

4.1 Simulerad positionering

Som tidigare nämnts används i detta examensarbete tillgängliga algoritmer för simulering av positionering och kartuppbyggnad med flera samverkande robotar, som utgångspunkt för positionering och kartuppbyggnad med verkliga data. Nedan följer en beskrivning av de positioneringsalgoritmer som används vid simuleringen. Positioneringsalgoritmen är ett extended kalmanfilter där alla robotar och objekts positioner är införda som tillstånd. Varje robot och objekt representeras med tre tillstånd xx, xy och xφ, där xx och xy representerar positionen i

ett jordfast koordinatsystem och xϕ representerar en vinkel i det jordfasta koordinatsystemet. Det kan påpekas att xϕ är vinkeln mellan en robots eller ett objekts riktning och den positiva x-axeln i det jordfasta koordinatsystemet. Vinkeln är inte begränsad till något intervall utan xϕ antas vara samma vinkel som xϕ+n2π. Om det i kalmanfiltret finns m antal robotar och n antal objekt får tillståndsvektorn x i (3-9) följande utseende

T on on y on x oj oj y oj x o o y o x rm rm y rm x ri ri y ri x r r y r x x x x x x x x x x x x x x x x x x x x ) , , , , , , , , , , , , , , , , , , , , ( ) ( ) ( ) ( ) ( ) ( ) ( ) 1 ( ) 1 ( ) 1 ( ) ( ) ( ) ( ) ( ) ( ) ( ) 1 ( ) 1 ( ) 1 ( ϕ ϕ ϕ ϕ ϕ ϕ " " " " = . Index r och o är införda för underlätta läsningen r indikerar att variabeln tillhör en robot och o indikerar att variabeln tillhör ett objekt. Det enda som skiljer robotar från objekt i kalmanfiltret är att robotar kan förflyttas och objekt antas stå stilla. I figur 4-1 visas ett scenario med tre objekt och två robotar.

Positionering

. .

Figur 4-1: Tre objekts och två robotars positioner i det jordfasta koordinatsystemet.

Vid simulering av positionering och kartuppbyggnad med flera samverkande robotar införs alla robotar och objekt som tillstånd i kalmanfiltret då

simuleringen startas. En kartuppbyggnadseffekt erhålls dock genom att osäkerheten om objektens position och vinkel sätts till ett stort värde.

Då tidsuppdateringar och mätuppdateringar görs i kalmanfiltret sker detta för en robot respektive en mätning åt gången. Detta gör att flera tidsuppdateringar och mätuppdateringar görs vid varje tidssteg i kalmanfiltret. Det skulle naturligtvis gå lika bra att endast utföra en tidsuppdatering och en mätuppdatering. I figur 4-2 visas ett flödesschema över hur positioneringsalgoritmen arbetar.

Figur 4-2: Flödesschema för positioneringsalgoritmen som används för simulering av positionering och kartbyggnad med flera samverkande robotar.

Positionering

. .

4.1.1 Systemuppdatering

Vid simulering beräknas en robots förflyttning mellan två tidssteg i kalmanfiltret utgående från styrsignalerna önskad förflyttning (us) och önskad

vinkelförändring (uφ). Detta kallas tidsuppdatering av tillstånden i kalmanfiltret

och den funktion som utför detta kallas systemfunktionen f i (3-9). Om det i kalmanfiltret finns m antal robotar och n antal objekt får modellen som beskriver robotarnas förflyttning följande utseende

) ( 1 ) ( ) ( 1 ) ( ) 1 ( 1 ) 1 ( ) ( 1 ) ( 1 ) ( 1 ) ( ) ( ) ( 1 ) ( 1 ) ( 1 ) ( ) ( ) 1 ( 1 ) 1 ( 1 ) 1 ( 1 ) 1 ( ) 1 ( ) , , ( ) , , ( ) , , ( on k on k oj k oj k o k o k rm k rm k rm k rm rm k ri k ri k ri k ri ri k r k r k r k r r k x x x x x x w u x f x w u x f x w u x f x − − − − − − − − − − − − = = = = = = = = = = # # # # # # # # . ( 4-1)

Eftersom objekten antas vara stilla är deras positioner oförändrade vid tidpunkt k och k-1. Systemfunktionen beräknar med hjälp av styrsignaler robotarnas nya estimerade positioner. En delfunktion till systemfunktionen för robot ri har följande utseende = = _{− − − −} − (ˆ , ) ˆ( ) ( ) _{1 1 1 1} 1 ri k k k k ri k k f x u x

(

)

(

)

(

) (

)

=                         ₋ + = − − − − − − − − − − − − − − − − ) ( 1 1 ) ( 1 1 ) ( 1 1 ) ( 1 1 ) ( 1 1 ) ( 1 1 ) ( 1 1 ) ( 1 1 1 0 0 0 ˆ cos ˆ sin 0 ˆ sin ˆ cos ˆ ri k k ri k k ri k k ri k k ri k k ri k k ri k k ri k k y x x x x x x δϕ δ δ ϕ ϕ ϕ ϕ                       − +               = − − − − − − − − − − − − − − − − − − − − − − 1 1 , 1 1 , 1 1 , 1 1 1 1 , 1 1 , 1 1 ) ( 1 1 ) ( 1 1 ) ( 1 1 ) ( 1 1 ) sin( 2 ) sin( , ) ˆ ( ˆ ˆ ˆ k k k k k k k k s k k k k k k s ri k k ri k k ri k k y ri k k x u u u u u u u x M x x x ϕ ϕ ϕ ϕ ϕ ϕ ϕ _δ (4-2) där

( )

( )

( )

( )

1 0 0 0 cos sin 0 sin cos ) (           − = a a a a a M . ( 4-3)

Funktionen (4-2) överför den önskade förflyttningen till en förflyttning i det jordfasta koordinatsystemet. Detta görs genom att robotens relativa förflyttning,

δx, δy och δφ, roteras till det jordfasta koordinatsystemet och adderas med

robotens position i det jordfasta koordinatsystemet. Den relativa förflyttningen beräknas med hjälp av geometrin som visas i figur 4-3

Figur 4-3: En robots relativa förflyttning utgående från styrsignalerna. Den punkt som representerar roboten är fäst på axelcentrum.

För att kunna beräkna den relativa förflyttningen beräknas först det kortaste avståndet mellan robotens nya och gamla position

) 2 sin( 2 ) 2 sin( 2 ϕ ϕ ϕ u u u u L L_d = _rc = s . (4-4)

Geometrin som visas i figur 2-3 och ekvation (4-4) ger

) sin( ) 2 cos( ) 2 sin( 2 ) 2 cos( _ϕ ϕ ϕ ϕ ϕ ϕ δ u u u u u u u u L x= _d = s = s ( 4-5) och ) 2 ( sin 2 ) 2 sin( 2 ϕ ϕ ϕ δ u u u u L y= d = s . ( 4-6)

Positionering

. .

Den total relativa förflyttningen ges av

                  =           ϕ ϕ ϕ ϕ ϕ δϕ δ δ u u u u u u u y x s s ) 2 ( sin 2 ) sin( 2 _{. ( 4-7)}

Om vagnen rör sig på en rät linje kan numeriska problem uppstå eftersom

uφ = 0 ger division med noll. Genom att använda gränsvärdet

0 sin då 1 sin _{→ →} a a a , ( 4-8)

som beskrivs i [22], erhålls

0. då 0 0 →           →           ϕ δϕ δ δ u u y x _s ( 4-9)

För att utföra prediktion av tillståndens kovariansmatris P behövs Fk som är en

linjarisering av fk. d.v.s. = ∂ ∂ = − − , ,0 ˆ_{kk 1}u_{k 1} x k k k x f F                                               ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ = " " " " " # # # # # # # # # # " " " " # # # # # # # # # # " " " " " " " " # # # # # # # # # # " " " " # # # # # # # # # # " " " " ) ( ) ( ) ( ) ( ) 1 ( ) ( ) ( ) ( ) ( ) ( ) 1 ( ) ( ) ( ) ( ) ( ) ( ) 1 ( ) ( ) ( ) ( ) ( ) ( ) 1 ( ) ( ) ( ) 1 ( ) ( ) 1 ( ) 1 ( ) 1 ( ) ( ) 1 ( ) ( ) 1 ( ) 1 ( ) 1 ( ) ( ) ( ) ( ) ( ) 1 ( ) ( ) ( ) ( ) ( ) ( ) 1 ( ) ( ) ( ) ( ) ( ) ( ) 1 ( ) ( ) ( ) ( ) ( ) ( ) 1 ( ) ( ) ( ) 1 ( ) ( ) 1 ( ) 1 ( ) 1 ( ) ( ) 1 ( ) ( ) 1 ( ) 1 ( ) 1 ( on on k oj on k o on k rm on k ri on k r on k on oj k oj oj k o oj k rm oj k ri oj k r oj k on o k oj o k o o k rm o k ri o k r o k on rm k oj rm k o rm k rm rm k ri rm k r rm k on ri k oj ri k o ri k rm ri k ri ri k r ri k on r k oj r k o r k rm r k ri r k r r k x f x f x f x f x f x f x f x f x f x f x f x f x f x f x f x f x f x f x f x f x f x f x f x f x f x f x f x f x f x f x f x f x f x f x f x f

                        ∂ ∂ = 1 0 0 0 1 0 0 1 0 0 0 1 ) ( ) ( ) ( " " % $ # # # # $ % " " ri ri k ri k x f F . ( 4-10)

Betraktar den partiella derivatan

=                       ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ = ∂ ∂ ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ri ri ri y ri ri x ri ri ri y ri y ri y ri x ri y ri ri x ri y ri x ri x ri x ri ri k x f x f x f x f x f x f x f x f x f x f ϕ ϕ ϕ ϕ ϕ ϕ 1 0 0 ˆ sin ˆ cos 1 0 ˆ cos ˆ sin 0 1 ) ( 1 1 ) ( 1 1 ) ( 1 1 ) ( 1 1 ) ( 1 1 ) ( 1 1 ) ( 1 1 ) ( 1 1                       −             −       − = _{− − − − − − − −} − − − − − − − − ri k k ri k k ri k k ri k k ri k k ri k k ri k k ri k k x y x x x y x x ϕ ϕ ϕ ϕ δ δ δ . (4-11) ) (ri k

F erhålls sedan genom att sätta in ekvation (4-11) i ekvation (4-10).

4.1.2 Processbrus

Processbruset modellerar osäkerheten för robotarnas förflyttning. Vid simulering införs processbruset som brus på styrsignalerna till robotarna. Bruset på

styrsignal uφ betecknas wφ och bruset på styrsignal us betecknas ws. Variansen för

bruset adderat till styrsignal uφ betecknas σ och variansen för bruset adderat till ϕ2

styrsignal us betecknas σ . Eftersom de båda brusen antas vara oberoende får s2

kovariansmatrisen för processbruset till robot ri följande utseende

        = ₂ 2 ) ( 0 0 ϕ σ σs ri k Q . ( 4-12)

Då tidsuppdateringen i kalmanfiltret utförs för robot ri står de andra robotarna stilla. Detta gör att den totala kovariansmatrisen,

[

_on

]

T k oj k o k rm k ri k r k k Q Q Q Q Q Q Q = ( 1) " ( ) " ( ) ( 1) " ( ) " ( ) , för processbruset kan skrivas som

Positionering . .

[

_ri

]

T k k Q Q = 0 " 0 ( ) 0 " 0 ( 4-13)

då tidsuppdateringen görs för robot ri i kalmanfiltret. För att utföra

tiduppdateringen för robot ri i kalmanfiltret behövs linjariseringen av f kring de två brusvariablerna = ∂ ∂ = − − , ,0 ˆ_{kk 1} u_{k 1} x k k k w f W =                                                 ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ = " " " " " # # # # # # # # # # " " " " # # # # # # # # # # " " " " " " " " # # # # # # # # # # " " " " # # # # # # # # # # " " " " ) ( ) ( ) ( ) ( ) 1 ( ) ( ) ( ) ( ) ( ) ( ) 1 ( ) ( ) ( ) ( ) ( ) ( ) 1 ( ) ( ) ( ) ( ) ( ) ( ) 1 ( ) ( ) ( ) 1 ( ) ( ) 1 ( ) 1 ( ) 1 ( ) ( ) 1 ( ) ( ) 1 ( ) 1 ( ) 1 ( ) ( ) ( ) ( ) ( ) 1 ( ) ( ) ( ) ( ) ( ) ( ) 1 ( ) ( ) ( ) ( ) ( ) ( ) 1 ( ) ( ) ( ) ( ) ( ) ( ) 1 ( ) ( ) ( ) 1 ( ) ( ) 1 ( ) 1 ( ) 1 ( ) ( ) 1 ( ) ( ) 1 ( ) 1 ( ) 1 ( on on k oj on k o on k rm on k ri on k r on k on oj k oj oj k o oj k rm oj k ri oj k r oj k on o k oj o k o o k rm o k ri o k r o k on rm k oj rm k o rm k rm rm k ri rm k r rm k on ri k oj ri k o ri k rm ri k ri ri k r ri k on r k oj r k o r k rm r k ri r k r r k w f w f w f w f w f w f w f w f w f w f w f w f w f w f w f w f w f w f w f w f w f w f w f w f w f w f w f w f w f w f w f w f w f w f w f w f                         ∂ ∂ = 0 0 0 0 0 0 0 0 0 0 0 0 ) ( ) ( " " % $ # # # # $ % " " ri ri k w f _{, ( 4-14)} där

(

( ₁) ( (_,) ₁)( ( ₁) ( ₁))

)

( (_,) ₁). ) ( 1 ) ( 1 ) ( ri k ri k ri k ri k ri k ri k ri k ri x M w u x M x w w f − − − − − − − = + + ∂ ∂ = ∂ ∂ ϕ ϕ

4.1.3 Mätmodellen

Vid simulering av mätningar gör en robot en mätning på ett objekt åt gången, på detta sätt undviks associeringsproblemet helt vid simulering. I

positioneringsalgoritmen uppdateras filtret med en mätning åt gången. Då en mätuppdatering ska göras med en mätning på objekt oj utförd av robot ri kan (3-10) skrivas på följande sätt ) , ( ) , , (zx(oj) zx(oj) zϕ(oj) T =h(ri,oj) x v . ( 4-15)

Vid simulering genereras mätningarna i det jordfasta koordinatsystemet vilket ger att ) ( ) ( ) , (rioj _{(xˆ) xˆ} oj _xˆ ri h = − ( 4-16) och = ∂ ∂ = ∂ ∂ = − − ˆ ,0 ) , ( 0 , ˆ ) , ( , 1 1 xk k k k x oj ri x k oj ri k _x h x h H =         ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ = (ri_(r,₁oj₎) (ri_(ri,oj₎) (₍ri_rm,oj₎) (ri_(o,₁oj₎) (ri_(oj,oj₎) (ri_(on,oj₎) x h x h x h x h x h x h " " " "         ∂ ∂ ∂ ∂ = 0 0 0 0 _{( )} 0 0 ) , ( ) ( ) , ( " " " oj oj ri ri oj ri x h x h ( 4-17) där           =                     ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ = ∂ ∂ 1 0 0 0 1 0 0 0 1 ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) , ( ri oj y oj x oj y oj y y oj x y oj x oj y x oj x x oj oj ri x h x h x h x h x h x h x h x h x h x h ϕ ϕ ϕ ϕ ϕ ϕ ( 4-18) och           − − − =                     ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ = ∂ ∂ 1 0 0 0 1 0 0 0 1 ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) , ( ri ri y ri x ri y ri y y ri x y ri x ri y x ri x x ri oj ri x h x h x h x h x h x h x h x h x h x h ϕ ϕ ϕ ϕ ϕ ϕ . ( 4-19)

Positionering

. .

Linjariseringen av h erhålls genom insättning av (4-18) och (4-19) i (4-17).

4.2 Positionering med verklig data

För att verklig data från den svepande lasern ska kunna införas i

positioneringsalgoritmerna för simulerad positionering måste en del ändringar och tillägg göras. Följande saker behövdes ändras eller läggas till:

Systemfunktionen

Systemfunktionen för de simulerade robotarna passar inte den

tillgängliga farkosten. Systemfunktionen för de simulerade robotarna förutsätter att man har tillgång till styrsignalerna för robotarna. Då farkosten saknar styrsignaler används farkostens beräknade förflyttning som ersättning för styrsignalerna.

Processbrus

I processbrusmodellen för de simulerade robotarna antas bruset på styrsignalerna vara konstant. För att detta ska vara en bra brusmodell bör robotarna röra sig med konstant hastighet. Eftersom hastigheten varierar under datainsamlingen införs en ny brusmodell.

Mätmodellen

Den mätmodell som används för de simulerade mätningarna passar inte de mätningar som erhålls från den svepande lasern. Vid simulering anges en mätning som avståndet mellan roboten och objektet utryckt i det jordfasta koordinatsystemet. De mätningar som erhålls från lasern är avståndet mellan farkosten och objektet uttryckt i ett koordinatsystem som är fäst på farkosten och roterat med samma vinkel som farkosten är roterad med i förhållande till det jordfasta koordinatsystemet.

Lägga till nya objekt

De mätningar som inte går att associera med något befintligt objekt är antingen falska mätningar eller en mätning på ett objekt som inte är inlagt i filtret. Vid simulering läggs alla befintliga objekt in i filtret när simuleringen startas. Eftersom de verkliga objektens position är okänd på förhand används en funktion som lägger till ett nytt objekt första gången en mätning erhålls på det.

Associering

Vid simulering försöker en robot göra en mätning på ett objekt och vissa kriterier avgör om den lyckas eller ej. I verkligheten extraheras

mätningar ur laserdata, dessa mätningar måste sedan associeras med de objekt som finns i filtret.

Extraktion av objekt från laserdata

Slutligen behövs metoder för att extrahera objekt ur mätdata. De objekt som extraheras ur laserdata är linjesegment och linjer.

I figur 4-4 visas ett flödesschema för den modifierade positioneringsalgoritm som används för positionering av farkosten med hjälp av verklig data.

Figur 4-4 : Flödesschema för positioneringsalgoritm som används för kartbyggnad och positionering med objekt extraherade ur laserdata.