Positionering och kartuppbyggnad med …19081/FULLTEXT01.pdf · Det finns flera olika metoder för...

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:

Tekn. Dr. Mikael Norrlöf, ISY

Avdelning, Institution Division, Department

Institutionen för Systemteknik 581 83 LINKÖPING

Datum 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 positionering, kartuppbyggnad, avståndsmätande laser, extended kalmanfilter, kalmanfilter

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 Introduktion .................................................................................................1

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.3 Inställning av Houghtransformen................................................49

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

8 Referenser..................................................................................................63

Introduktion . .

1

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 . .

2

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.

Introduktion . .

3

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 . .

4

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. Lrc Radie från farkostens rotationscentrum till axelcentrum.

Introduktion . .

5

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. z, z Mätningar och förväntade mätningar.

Sensorplattformen . .

6

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.


7

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.


8

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


9

=

),,,,(),,,,(

arvrhvh

arvrhvhs

uuuuueuuuuues

θθϕ

θθ

δϕδ

. ( 2-1)

Några grundläggande samband ges av ekvationerna nedan.

δϕθ rvvrvv Luus == ( 2-2) δϕθ rhhrhh Luus == ( 2-3)

δϕδ rcLs = ( 2-4)

arhrv uLL =− ( 2-5)

2rvrh

rcLL

L+

= . ( 2-6)

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

( )hrhvrva

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

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

( ) ( )hrhvrvrhrv uuuuLLs θθδϕδϕδ +=+=21

21 . ( 2-8)

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

( )

( )

−

+=

=

hrhvrva

hrhvrv

arvrhvh

arvrhvhs

uuuuu

uuuu

uuuuueuuuuues

θθ

θθ

θθϕ

θθ

δϕδ

121

),,,,(),,,,(

. ( 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.


10

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.


11

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


12

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


13

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 resurshanteraren.


14

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å nätverksdatorer hämtar data från samma sensor måste två nätverksprogram


15

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 . .

16

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

111 −−− ++= kkkk wGuFxx ( 3-1)

kkk vHxz += . ( 3-2) Där matrisen G anger hur insignalerna påverkar tillstånden. H är mätmatrisen och F är systemmatrisen. Processbruset som betecknas ),0(~ kk QNw och mätbruset som betecknas ),0(~ kk RNv antas vara oberoende av varandra. För en sådan modell ges tidsuppdateringen i kalmanfiltret av

1111 ˆˆ −−−− += kkkkk GuxFx ( 3-3)

1111 −−−− += kT

kkkk QFFPP . ( 3-4)

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

kT

kkk RHHPS += −1| ( 3-5) 1

1|−

−= kT

kkk SHPK ( 3-6)

]ˆ[ˆˆ 1|1|| −− −+= kkkkkkkk xHzKxx ( 3-7)

1|| ][ −−= kkkkk PHKIP . ( 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


17

linjariseringen. En olinjär modell där xk representerar de sanna tillstånden vid tidssteg k och uk representerar insignalerna kan skrivas som

),,( 111 −−−= kkkk wuxfx ( 3-9) ),( kkk vxhz = . ( 3-10)

Tidsuppdateringen för ett EKF ges av )0,,ˆ(ˆ 11|11| −−−− = kkkkk uxfx ( 3-11)

Tkkk

Tkkkkkk WQWFPFP 11|11| −−−− += . ( 3-12)

Där

0,,ˆ 11 −−∂∂

=kkk uxk

k wfW ( 3-13)

och

0,,ˆ 11 −−∂∂

=kkk uxk

k xfF . ( 3-14)

Mätuppdateringen för ett EKF ges av Tkkk

Tkkkkk VRVHPHS += −1| ( 3-15)

11|

−−= k

Tkkkk SHPK ( 3-16)

)]0,ˆ([ˆˆ 1|1|| −− −+= kkkkkkkk xhzKxx ( 3-17)

1|1|| −− −= kkkkkkkk PHKPP . ( 3-18) Där

0,ˆ 1−∂∂

=kkxk

k xhH ( 3-19)

och

0,ˆ 1−∂∂

=kkxk

k vhV . ( 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.


18

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

Tkk zzSzzzd |11

1|1

2 ˆˆ)( ++−

+ −−= . ( 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

=)ˆ,()ˆ,(

)ˆ,()ˆ,(),(

21

2

12

112

nmm

n

zzdzzd

zzdzzdnmD . ( 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.

Positionering . .

19

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

Tonony

onx

ojojy

ojx

ooy

ox

rmrmy

rmx

ririy

rix

rry

rx

xxxxxxxxx

xxxxxxxxxx

),,,,,,,,,

,,,,,,,,,,,()()()()()()()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 . .

20

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.

Positionering . .

21

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

Positionering . .

22

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(

),,(

),,(

),,(

onk

onk

ojk

ojk

ok

ok

rmk

rmk

rmk

rmrmk

rik

rik

rik

ririk

rk

rk

rk

rrk

xx

xx

xx

wuxfx

wuxfx

wuxfx

−

−

−

−−−

−−−

−−−

=

=

=

=

=

=

=

=

=

=

. ( 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

== −−−−− ),ˆ(ˆ 1111)()(

1 kkkkriri

kk uxfx

( ) ( )( ) ( ) =

−

+=

−−

−−

−−

−−−−

−−−−

−−)(

11

)(11

)(11

)(11

)(11

)(11

)(11

)(11

1000ˆcosˆsin

0ˆsinˆcos

ˆri

kk

rikk

rikk

rikk

rikk

rikk

rikk

rikk y

x

xx

xx

x

δϕ

δ

δ

ϕϕ

ϕϕ

−+

=

−−

−−−−

−−

−−−−

−−

−−

−−

−−

−−

11,

11,11,

11

11,11,

11

)(11

)(11

)(11

)(11

)sin(2

)sin(,

)ˆ(

ˆ

ˆ

ˆ

kk

kkkk

kks

kkkk

kks

rikk

rikk

rikky

rikkx

u

uu

u

uu

u

xM

x

x

x

ϕ

ϕϕ

ϕϕ

ϕ

ϕ δ

(4-2)

där

Positionering . .

23

( ) ( )( ) ( )

1000cossin0sincos

)(

−= aa

aaaM . ( 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 ϕ

ϕ

ϕ uuuu

LL srcd == . (4-4)

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

)sin()2

cos()2

sin(2)2

cos( ϕϕ

ϕϕ

ϕ

ϕδ uuuuu

uuu

Lx ssd === ( 4-5)

och

)2

(sin2)2

sin( 2 ϕ

ϕ

ϕδu

uuu

Ly sd == . ( 4-6)

Positionering . .

24

Den total relativa förflyttningen ges av

=

ϕ

ϕ

ϕ

ϕϕ

δϕδδ

u

uuu

uuu

yx

s

s

)2

(sin2

)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

0sin då 1sin→→ a

aa , ( 4-8)

som beskrivs i [22], erhålls

0. då 00 →

→

ϕ

δϕδδ

uu

yx 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,,ˆ 11 kkk uxk

kk x

fF

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

=

)(

)(

)(

)(

)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

onk

oj

onk

o

onk

rm

onk

ri

onk

r

onk

on

ojk

oj

ojk

o

ojk

rm

ojk

ri

ojk

r

ojk

on

ok

oj

ok

o

ok

rm

ok

ri

ok

r

ok

on

rmk

oj

rmk

o

rmk

rm

rmk

ri

rmk

r

rmk

on

rik

oj

rik

o

rik

rm

rik

ri

rik

r

rik

on

rk

oj

rk

o

rk

rm

rk

ri

rk

r

rk

xf

xf

xf

xf

xf

xf

xf

xf

xf

xf

xf

xf

xf

xf

xf

xf

xf

xf

xf

xf

xf

xf

xf

xf

xf

xf

xf

xf

xf

xf

xf

xf

xf

xf

xf

xf

eftersom en robots position uppdateras åt gången, se flödesschema figur 4-2, blir

Positionering . .

25

∂

∂=

1000

10

010

001

)(

)()(

ri

rikri

kx

fF . ( 4-10)

Betraktar den partiella derivatan

=

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

=∂

∂

)(

)(

)(

)(

)(

)(

)(

)(

)(

)(

)(

)(

)(

)(

)(

)(

)(

)(

)(

)(

ri

ri

riy

ri

rix

ri

ri

riy

riy

riy

rix

riy

ri

rix

riy

rix

rix

rix

ri

rik

x

f

x

f

x

fx

f

x

f

x

fxf

xf

xf

x

f

ϕ

ϕϕϕ

ϕ

ϕ

100

ˆsinˆcos10

ˆcosˆsin01

)(11

)(11

)(11

)(11

)(11

)(11

)(11

)(11

−

−

−

=−−−−−−−−

−−−−−−−−

rikk

rikk

rikk

rikk

rikk

rikk

rikk

rikk

xyxx

xyxx

ϕϕ

ϕϕ

δ

δδ

. (4-11)

)(rikF 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 2

ϕσ och variansen för bruset adderat till

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

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

= 2

2)(

00

ϕσσ sri

kQ . ( 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,

[ ]Tonk

ojk

ok

rmk

rik

rkk QQQQQQQ )()()1()()()1(= ,

för processbruset kan skrivas som

Positionering . .

26

[ ]Trikk QQ 0000 )(= ( 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,,ˆ 11 kkk uxk

kk w

fW

=

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

=

)(

)(

)(

)(

)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

onk

oj

onk

o

onk

rm

onk

ri

onk

r

onk

on

ojk

oj

ojk

o

ojk

rm

ojk

ri

ojk

r

ojk

on

ok

oj

ok

o

ok

rm

ok

ri

ok

r

ok

on

rmk

oj

rmk

o

rmk

rm

rmk

ri

rmk

r

rmk

on

rik

oj

rik

o

rik

rm

rik

ri

rik

r

rik

on

rk

oj

rk

o

rk

rm

rk

ri

rk

r

rk

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

fw

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

∂

∂=

0000

00

000

000

)(

)(

ri

rik

w

f , ( 4-14)

där

( ) ).())(( )(1,

)(1

)(1

)(1,

)(1)(

1)(1

)(rik

rik

rik

rik

rikri

kri

k

rixMwuxMx

wwf

−−−−−−−

=++∂

∂=

∂

∂ϕϕ

Positionering . .

27

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

),(),,( ),()()()( vxhzzz ojriTojojx

ojx =ϕ . ( 4-15)

Vid simulering genereras mätningarna i det jordfasta koordinatsystemet vilket ger att

)()(),( ˆˆ)ˆ( riojojri xxxh −= ( 4-16) och

=∂

∂=

∂∂

=−− 0,ˆ

),(

0,ˆ

),(

,11 kxkk xk

ojri

xk

ojrik x

hxhH

=

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂=

)(

),(

)(

),(

)1(

),(

)(

),(

)(

),(

)1(

),(

on

ojri

oj

ojri

o

ojri

rm

ojri

ri

ojri

r

ojri

xh

xh

xh

xh

xh

xh

∂

∂

∂

∂= 000000

)(

),(

)(

),(

oj

ojri

ri

ojri

xh

xh ( 4-17)

där

=

∂

∂

∂

∂

∂

∂∂

∂

∂

∂

∂

∂∂

∂

∂

∂

∂

∂

=∂

∂

100010001

)()()(

)()()(

)()()(

)(

),(

riojy

ojx

ojy

ojy

yoj

x

y

ojx

ojy

xoj

x

x

oj

ojri

x

h

x

h

x

hx

h

x

h

x

hxh

xh

xh

xh

ϕ

ϕϕϕ

ϕ

ϕ

( 4-18)

och

−−

−=

∂

∂

∂

∂

∂

∂∂

∂

∂

∂

∂

∂∂

∂

∂

∂

∂

∂

=∂

∂

100010001

)()()(

)()()(

)()()(

)(

),(

ririy

rix

riy

riy

yri

x

y

rix

riy

xri

x

x

ri

ojri

x

h

x

h

x

hx

h

x

h

x

hxh

xh

xh

xh

ϕ

ϕϕϕ

ϕ

ϕ

. ( 4-19)

Positionering . .

28

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.

Positionering . .

29

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

Positionering . .

30

4.2.1 Systemuppdatering 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. Då styrsignalerna i (4-2) ersätts med den beräknade förflyttningen från (2-9) får systemfunktionen för robot ri får följande utseende

=−− )),,,,(,ˆ( 11)(

arvrhvhkkri uuuuuexf θθ

−+

= −−

−−

−−

−−

δϕ

δϕδϕδ

δϕδϕδ

ϕ

ϕ

)sin(2

)sin(

)ˆ(

ˆ

ˆ

ˆ)(

11)(

11

)(11

)(11

s

s

xM

x

x

xri

kkri

kk

rikky

rikkx

, (4-20)

och =−− )),,,,(,ˆ( 11

)(arvrhvhkk

ri uuuuuexf θθ

−+

= −−

−−

−−

−−

00 )ˆ(

ˆ

ˆ

ˆ)(

11)(

11

)(11

)(11 s

xM

x

x

xri

kkri

kk

rikky

rikkx δ

ϕ

ϕ

(4-21)

om δφ=0.

4.2.2 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. Denna brusmodell är hämtad ur [12]. Brusmodellen lägger till brus på tillryggalagd sträcka (δs) och brus på vinkelförändringen (δφ). De designparametrar som går att variera i brusmodellen är kDs, kDφ1och kDφ2. I denna brusmodell är T

TOT WQWQ = och storleken på varianserna för w antas bero av den sträcka farkosten förflyttats och farkostens vinkelförändring. Varianserna till systembruset w är enligt modellen

Positionering . .

31

sk Dss δσ =2 ( 4-22) och

δϕδδδ

δϕδ

δδϕδσ

ϕϕϕϕ

ϕϕϕϕ

)()(

}{

)()(

)()(2

rksrkk

srksk

DDs

D

Ds

D

=+=

===+=. ( 4-23)

Eftersom w är en funktion av δs och δφ så kommer TOTQ att vara en funktion av δs och δφ. Detta hanteras i [12] så att intervaller k-1 till k delas in N steg och

iri

TOTQ )( beräknas för varje steg. TOTQ fås sedan genom att låta N gå mot

oändligheten och summera ∑=

N

ii

riTOTQ

1

)( . Resultatet av dessa beräkningar är

enligt [12]

)))ˆ2sin()ˆ2)(sin(ˆcos(2)ˆ(cos)(()(

)2

)ˆ2sin()ˆ2sin()(())((

21

)()(1

)(1

)(1

22

)()(12)(

11,,

rik

rik

rik

rikD

rik

rik

DDsri

kTOT

xxxxsignrk

xxsignskrkQ

ϕϕϕϕϕ

ϕϕϕ

δϕδϕδ

δϕδϕδϕδ

δ

−−+

−++=

+++

+

))ˆˆcos()ˆ2cos()ˆ2sin(2

)(()(

)2

)ˆ2cos()ˆ2cos()(())((

41

)(1

)()(1

)(1

2

)()(12)(

12,,

rik

rik

rik

rikD

rik

rik

DDsri

kTOT

xxxxsignrk

xxsignrkrkQ

+++

+

+−++

−++−=

ϕϕϕϕϕ

ϕϕϕ

δϕδϕδ

δϕδϕδδ

))ˆsin()ˆsin()ˆcos(( )()(1

)(1

)(13,,

rik

rik

rikD

rikTOT xxxrkQ ϕϕϕϕ δϕδ +−=

++

)))ˆcos()ˆ)(cos(ˆsin(2)ˆ(sin)(()(

)2

)ˆ2sin()ˆ2sin()(())((

21

)()(1

)(1

)(1

22

)()(12)(

22,,

rik

rik

rik

rikD

rik

rik

DDsri

kTOT

xxxxsignrk

xxsignrkrkQ

ϕϕϕϕϕ

ϕϕϕ

δϕδϕδ

δϕδϕδδ

−++

−−+=

+++

+

))cos()ˆcos()ˆsin(( )(1

)(1

)(23,, δϕδϕδ ϕϕϕ −+=

++ri

kri

kDri

kTOT xxrkQ ( )

, ,33ri

TOT k DQ k ϕ δϕ= .

Om δφ=0 ger ovanstående ekvationer numeriska problem )(riTOTQ ges då istället av

))ˆ(sin31)ˆcos(( )(22)()()(

11,,ri

ks

Dri

kDsri

kTOT xskxksQ ϕϕϕ δδ +=

)ˆ2sin()31(

21 )(2)()(

12,,ri

ks

DDsri

kTOT xskksQ ϕϕδδ +=

)ˆcos(21 )()()(

13,,ri

ks

Dri

kTOT xsksQ ϕϕδδ−=

Positionering . .

32

))ˆ(cos31)ˆsin(( )(22)()()(

22,,ri

ks

Dri

kDsri

kTOT xskxksQ ϕϕϕ δδ +=

)ˆcos(21 )()()(

23,,ri

ks

Dri

kTOT xsksQ ϕϕδδ=

)()(33,,

sD

rikTOT ksQ ϕδ= .

Brusmodellen införs i kalmanfiltret genom att

∂

∂

∂

∂=

0000

00

000

000

)(

)()(

)(

)()()()(

ri

rikri

ri

rikririri

w

fQ

w

fWQW ( 4-24)

i den simulerade tidsuppdateringen ersätts med

=

0000

00

000

000

)()()()( riTOT

ririri QWQW . ( 4-25)

4.2.3 Mätmodellen 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. Mätfunktionen h i (3-10) är en funktion som transformerar objektets skattade position i det jordfasta koordinatsystemet till det koordinatsystem i vilket mätningen genererats. En förväntad mätning av objekt oj gjord av robot ri uttryckt i det koordinatsystem som är fäst på robot ri betecknas Tojriojri

yojri

x zzz )ˆ,ˆ,ˆ( ),(),(),(ϕ .

Positionering . .

33

Figur 4-5: Illustration av förväntad mätning på objekt oj gjord av robot ri i det jordfasta koordinatsystemet.

Figur 4-6: Illustration av förväntad mätning på objekt oj gjord av robot ri i det koordinatsystem som är fäst på robot ri. Transformationen av tillstånden för objekt oj till koordinatsystemet som är fäst på robot ri utförs av

−

−+−−

−+−

=

)()(

)()()()()()(

)()()()()()(

),(

),(

),(

)cos()()sin()()sin()()cos()(

rioj

ririy

ojy

ririx

ojx

ririy

ojy

ririx

ojx

ojri

ojriy

ojrix

xxxxxxxx

xxxxxx

hhh

ϕϕ

ϕϕ

ϕϕ

ϕ

. ( 4-26)

Positionering . .

34

Mätekvationen (4-26) subtraherar först robotens tillstånd från objektets efter detta görs en rotation till koordinatsystemet som är fäst på roboten. Mätekvationen är följaktligen

))(()(ˆ )()()(),(),( riojriojriojri xxxMxhz −== ϕ . Nedan följer beräkningen av Hk till (4-26) som används vid mätuppdateringen.

=∂

∂=

∂∂

=

−− 0,ˆ

),(

0,ˆ

),(

,11 kxkk xk

ojri

xk

ojrik x

hxhH

=

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂=

)(

),(

)(

),(

)1(

),(

)(

),(

)(

),(

)1(

),(

on

ojri

oj

ojri

o

ojri

rm

ojri

ri

ojri

r

ojri

xh

xh

xh

xh

xh

xh

∂

∂

∂

∂= 000000

)(

),(

)(

),(

oj

ojri

ri

ojri

xh

xh . ( 4-27)

Betraktar den partiella derivatan med avseende på robot ri

( )=−∂

∂=

∂

∂ )()()()()(

),()( riojri

riri

ojrixxxM

xxh

ϕ

=

∂

∂

∂

∂

∂

∂∂

∂

∂

∂

∂

∂∂

∂

∂

∂

∂

∂

=

)()()(

)()()(

)()()(

ririy

rix

riy

riy

yri

x

y

rix

riy

xri

x

x

x

h

x

h

x

hx

h

x

h

x

hxh

xh

xh

ϕ

ϕϕϕ

ϕ

ϕ

=

−−−−−−

−+−−−−

=100

)sin()()cos()()cos()sin()cos()()sin()()sin()cos(

)()()()()()()()(

)()()()()()()()(

ririy

ojy

ririx

ojx

riri

ririy

ojy

ririx

ojx

riri

xxxxxxxxxxxxxxxx

ϕϕϕϕ

ϕϕϕϕ

−−

−

+−=000

)(00)(00

)()( )()(

)()(

)()( rix

ojx

riy

ojy

riri xxxx

xMxM ϕϕ . ( 4-28)

Betraktar den partiella derivatan med avseende på objekt oj

Positionering . .

35

)( )(

)()()(

)()()(

)()()(

)(

),(ri

ojojy

ojx

ojy

ojy

yoj

x

y

ojx

ojy

xoj

x

x

oj

ojrixM

xh

xh

xh

xh

xh

xh

xh

xh

xh

xh

ϕ

ϕ

ϕϕϕ

ϕ

ϕ

=

∂

∂

∂

∂

∂

∂∂

∂

∂

∂

∂

∂∂∂

∂∂

∂∂

=∂∂ . ( 4-29)

Ekvation (4-27) - (4-29) ger att

.000000

)(000000

000)(00

)(00)()(

000000

)()()(

)()(

)()(

),(

−−

−

+−=

=

ririx

ojx

riy

ojy

riri

ojrik

xMxxxx

xMxM

H

ϕϕϕ

4.2.4 Associering av objekt I detta examensarbete används en variant av ett NNSF [20] för att associera mätningar med objekt. I ett NNSF används den viktade normen för innovationen som avståndsmått. Detta avstånd kallas även mahalanobisavståndet och ges av

[ ] [ ]kkkT

kk zzSzzzd |111

|12 ˆˆ)( ++

−+ −−= . ( 4-30)

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

=)ˆ,()ˆ,(

)ˆ,()ˆ,(),(

21

2

12

112

nmm

n

zzdzzd

zzdzzdnmD . ( 4-31)

I ett NNSF utförs associering mellan de mätningar och de objekt som minimerar det totala mahalanobisavståndet. Att finna det totala mahalanobisavståndet är ett beräkningskrävande optimeringsproblem. Komplexiteten på optimeringsproblemet kan minskas genom att de osannolika mahalanobisavstånden sorteras bort på statistisk väg. I detta examensarbete minskas komplexiteten genom att en mätning associeras åt gången. Då en mätning associeras åt gången reduceras matrisen i (4-31) till en vektor

[ ])ˆ,()ˆ,()( 21

2niii zzdzzdnD = . ( 4-32)

Innan mahalanobisavståndet beräknas sorteras de objekt som ligger så långt från mätningen att de ej kan ha genererat den bort. Detta görs genom jämförelsen

max21

2),(2),( ))ˆ()ˆ(( rzzzz ojriyy

ojrixx <−+− . ( 4-33)

Att på detta sätt sortera bort orimliga mätningar har stor fördel då de objekt som ska associeras är linjer. Eftersom inga associationer av objekt, som ligger på ett större avstånd än rmax från mätningen sker, kommer linjer att segmenteras med

Positionering . .

36

längden rmax. Följaktligen kan rmax användas som designparameter för längden av linjeobjekt. Den skattade mätningen beräknas med mätekvationen

)0,ˆ(ˆˆˆ

),(

),(

),(

),(

kojri

ojri

ojriy

ojrix

xhzzz

=

ϕ

. ( 4-34)

Om villkoret ovan är uppfyllt kontrolleras att vinkelskillnaden mellan mätningen och den skattade mätningen är mindre än π. Om vinkelskillnaden är större än π korrigeras mätningens vinkel med en multipel av 2π så att vinkelskillnaden blir mindre än π. Detta görs genom

]2

[2)(

ππ ϕϕ

ϕϕ

ojzzzz

−+= . ( 4-35)

Hakparenteserna i funktionen ovan representerar heltalsfunktionen som avrundar sitt argument till närmaste heltalsvärd. Efter att mätningens vinkel korrigerats beräknas

kT

kk RHHPS += −1| , ( 4-36) där

0,ˆ

),(

1−∂

∂=

kkxk

ojri

xhH . ( 4-37)

Vidare beräknas mahalanobisavståndet mellan mätningen och den skattade mätningen på objekt oj gjord av robot ri, detta görs genom

−−−

−−−

= −

),(

),(

),(

1

),(

),(

),(

2

ˆˆˆ

ˆˆˆ

ojri

ojriy

ojrix

y

xT

ojri

ojriy

ojrix

y

x

j

zzz

zzz

Szzz

zzz

d

ϕϕϕϕ

. ( 4-38)

Mahalanobisavståndet beräknas för alla objekt som uppfyllde villkoret i (4-33) och de objekt som inte uppfyller villkoret tilldelas ett stort mahalanobisavstånd. Mätningen associeras sedan med det objekt som ger upphov till det lägsta mahalanobisavståndet. Vinkeln korrigeras ytterligare en gång eftersom den kan ändras då mahalanobisavståndet beräknas för de andra objekten. Att associera en mätning åt gången är snabbare och enklare än en metod som minimerar det totala mahalanobisavståndet för alla mätningar på en gång. Dock erhålls en del felassociationer som kan undvikas om alla mätningar associerades samtidigt. I figur (4-7) visas ett exempel när associering av en mätning åt gången ger upphov till en felassociering som skulle ha undvikits om båda mätningarna hade associerats samtidigt. Då en mätning åt gången associeras kommer både mätning ett och mätning två att associeras med objekt 2. Medan mätning ett hade associerats med objekt ett och mätning två hade associerats med objekt två, om associeringen av de båda mätningarna hade utförts samtidigt.

Positionering . .

37

Figur 4-7: Då en mätning associeras åt gången kommer både mätning ett och mätning två att associeras med objekt 2. Medan mätning ett hade associerats med objekt ett och mätning två hade associerats med objekt två om associeringen av de båda mätningarna hade utförts samtidigt Eftersom det kan förekomma mätningar på objekt som inte finns i filtret ska inte alltid associering göras med det objekt som har lägst mahalanobisavstånd. För att avgöra om det associeringsförslag som NNSF ger verkligen ska utföras granskas storleken av mahalanobisavståndet. Genom att sätta ett maxvärde för storleken på mahalanobisavståndet kan utrensning ske av de mätningar som inte ska associeras. I fall falska mätningar aldrig 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 gränsvärdet för mahalanobisavståndet ska vara för att rätt associering ska göras med en viss procents säkerhet. Den associeringsmetod som används i detta fall har fördelarna att den är enkel och snabb men den kan göra felassociationer i vissa situationer.

4.2.5 Lägga till nya objekt Då det inte går att associera en mätning med något objekt i filtret är det antingen en mätning på ett nytt objekt eller en felaktig mätning. Om det är en mätning på ett nytt objekt ska det nya objektet införas i filtret medan en felaktig mätning ska kasseras. För att upptäcka felaktiga mätningar antas att de falska mätningarna är dåliga mätningar på redan existerande objekt. Genom att höja gränsvärdet för mahalanobisavståndet i associeringsalgoritmen upptäcks dåliga mätningar på redan existerande objekt. De mätningar som finns kvar efter denna kontroll antas vara mätningar på nya objekt. Ett nytt objekt införs i filtret dels genom att dess position och vinkel införs i tillståndsvektorn x och dels genom att

Positionering . .

38

kovariansmatrisen P utökas med objektets osäkerhet och dess korskorrelation med osäkerheten för de redan existerande objekten och robotarna. En mätning är given i ett koordinatsystem som är fäst på den mätande roboten. Eftersom ett objekts position och vinkel lagras i ett jordfast koordinatsystem måste mätningen transformeras från koordinatsystemet som är fäst på roboten till det jordfasta koordinatsystemet. Detta utförs av funktionen

=

−−−

−−

+

=

=+

+

+

+

+

+

+

)1(

)1(

)1(

)()(

)()(

)(

)(

)(

)1,(

)1,(

)1,(

)1,(

1000)cos()sin(0)sin()cos(

),(oj

ojy

ojx

riri

riri

ri

riy

rix

ojri

ojriy

ojrix

onri

zzz

xxxx

xxx

ggg

zxg

ϕ

ϕϕ

ϕϕ

ϕϕ

+

++

−+

=+

++

++

)1()(

)()1()()1()(

)()1()()1()(

)cos()sin()sin()cos(

onri

riony

rionx

riy

riony

rionx

rix

zxxzxzxxzxzx

ϕϕ

ϕϕ

ϕϕ

( 4-39)

som först roterar mätningen och sedan adderar position och vinkel för den mätande roboten. Det nya objektets position och vinkel ges av

)1()()()1()1( )(),( +++ −+== onririonon zxMxzxgx ϕ . ( 4-40) För att illustrera proceduren föreställer vi oss en situation där systemet består av m robotar och n objekt. Innan ett nytt objekt lagts till har tillståndsvektorn följande utseende

Tonon

yon

xojoj

yoj

xoo

yo

x

rmrmy

rmx

ririy

rix

rry

rx

xxxxxxxxx

xxxxxxxxxx

),,,,,,,,,

,,,,,,,,,,,()()()()()()()1()1()1(

)()()()()()()1()1()1(

ϕϕϕ

ϕϕϕ=.

Efter det att det nya objektets tillstånd lagts till i tillståndsvektor har den följande utseende

Tonony

onx

onony

onx

ojojy

ojx

ooy

ox

rmrmy

rmx

ririy

rix

rry

rx

xxxxxxxxxxxx

xxxxxxxxxx

).,,,,,,,,,,,,

,,,,,,,,,,,()1()1()1()()()()()()()1()1()1(

)()()()()()()1()1()1(

+++

=

ϕϕϕϕ

ϕϕϕ

Eftersom objekten är extraherade från mätningar som är gjorda av en robot kommer det nya objektets osäkerhet att vara korrelerat med osäkerheten för roboten och osäkerheten för objekt som roboten gjort mätningar på. Korrelationen till roboten är uppenbar eftersom det nya objektets position i jordfasta koordinater är en funktion av robotens position och mätningen. Korrelationen med andra objekt är mer indirekt men följer från att robotens position delvis har bestämts genom mätningar på existerande objekt. Enligt [21] erhålls den expanderade kovariansmarisen Pexp av

=

ABBPP

Told

exp , ( 4-41)

där

Positionering . .

39

oldx PGB = ( 4-42) och

Tzz

Txoldx RGGGPGA += . ( 4-43)

Gx är jacobianen till g med avseende på tillståndet x och Gz är jacobianen till g med avseende på tillståndet z. Nedan följer beräkningen av Gx.

=∂

∂=

+

xgG

onri

x

)1,(

=

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂=

++++++

)(

)1,(

)(

)1,(

)1(

)1,(

)(

)1,(

)(

)1,(

)1(

)1,(

on

onri

oj

onri

o

onri

rm

onri

ri

onri

r

onri

xg

xg

xg

xg

xg

xg

∂

∂=

+

0000)(

)1,(

ri

ojri

xg

. ( 4-44)

Betraktar den partiella derivatan

=

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

=∂

∂

+++

+++

+++

+

)(

)1,(

)(

)1,(

)(

)1,(

)(

)1,(

)(

)1,(

)(

)1,(

)(

)1,(

)(

)1,(

)(

)1,(

)(

)1,(

ri

onri

riy

onri

rix

onri

ri

onriy

riy

onriy

rix

onriy

ri

onrix

riy

onrix

rix

onrix

ri

onri

x

g

x

g

x

gx

g

x

g

x

gx

gx

gx

g

xg

ϕ

ϕϕϕ

ϕ

ϕ

+

−−

= ++

++

100)sin()cos(10)cos()sin(01

)()1()()1(

)()1()()1(

riojy

riojx

riojy

riojx

xzxzxzxz

ϕϕ

ϕϕ

. ( 4-45)

Gx erhålls slutligen genom insättning av ekvation (4-45) i ekvation (4-44). Nedan följer beräkningen av Gz.

)( )(

)1(

)1,(

)1(

)1,(

)1(

)1,(

)1(

)1,(

)1(

)1,(

)1(

)1,(

)1(

)1,(

)1(

)1,(

)1(

)1,(

)1(

)1,(ri

on

onri

ony

onri

onx

onri

on

onriy

ony

onriy

onx

onriy

on

onrix

ony

onrix

onx

onrix

on

onri

z xM

z

g

z

g

z

gz

g

z

g

z

gz

gz

gz

g

zgG ϕ

ϕ

ϕϕϕ

ϕ

ϕ

−=

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

∂

=∂

∂=

+

+

+

+

+

+

+

+

+

+

+

+

+

+

+

+

+

+

+

+

. ( 4-46)

Extraktion av objekt ur laserdata . .

40

5 Extraktion av objekt ur laserdata De objekt som extraherats ur laserdata är linjesegmentsobjekt och linjeobjekt. Skillnaden mellan linjeobjekt och linjesegmentsobjekt är att linjesegmentsobjekt har en mätbar längd medan linjeobjekt har en odefinierad längd.

5.1 Linjesegmentsobjekt Ett linjesegment representeras som en position och en riktning i det jordfasta koordinatsystemet. Positionen representerar mittpunkten på linjesegmentet och riktningen representerar normalen till linjesegmentet. För att en serie mätpunkter i ett lasersvep ska klassas som en mätning på ett linjesegment får varken början eller slutet av linjesegmentet vara skymd.

5.1.1 Extraktion av linjesegment ur laserdata Då linjesegmentobjekt extraheras ur laserdata är det första steget att hitta de punkter i laserdata som utgör möjliga linjesegmentobjekt. Genom att använda den rekursiva algoritm som beskrivs nedan erhålls möjliga linjesegmentobjekt.

1. Skapa ett linjesegment av den första och den sista mätpunkten i lasermätningen.

2. Identifiera den mätning som avviker mest från linjen. 3. Om avvikelsen är mindre än den maximalt tillåtna avvikelsen är ett

linjesegment funnet. Om avvikelsen för mätningen avviker mer än den maximalt tillåtna avvikelsen så delas lasermätningen, vid den mätpunkt som avviker mest från linjen, i två nya delmätningar.

4. Funktionen anropar sig själv med de delmätningar som innehåller fler mätpunkter än tröskelvärdet.

Denna algoritm är hämtad från [23] och ger ett antal möjliga linjesegment. I figur 5-1 visas linjesegmentskandidater framtagna med den rekursiva algoritm som beskrivs ovan.


41

Figur 5-1 : Möjliga linjesegmentobjekt framtagna med en rekursiv algoritm.

Figur 5-2: Omgivningen från vilken de möjliga linjesegmentobjekten i figur 5-1 är extraherade.

5.1.2 Behandling av linjesegmentskandidater Denna algoritm för att ta fram linjesegmentskandidater översegmenterar ibland data, vilket kan bero på att någon mätpunkt är felaktig. De översegmenterade linjesegmenten slås samman om variansen för det sammanslagna linjesegmentet inte överstiger ett förutbestämt gränsvärde. När linjesegment erhålls från


42

linjesegmentsalgoritmen representeras de av första och sista mätningen i linjesegmentet. Denna beskrivning av linjesegment är inte optimal eftersom den ej använder alla mätningar som erhållits. För en bättre beskrivning av linjesegmentet beräknas med minsta kvadratmetoden en position och en riktning. Linjesegmentet representeras nu av en längd, en vinkel och en punkt som är fäst mitt på linjesegmentet.

Figur 5-3: Anpassning av linjesegmentet med minsta kvadratmetoden. Nästa steg är att avgöra huruvida ett linjesegment är giltigt. För att ett linjesegment ska vara giltigt måste både startpunkt och slutpunkt vara synliga. Det finns två anledningar till att inte hela linjesegmentet ses, dels att något i omgivningen skymmer delar av linjesegmentet, dels att någon del av linjesegmentet ligger utanför den sektor som lasern avsöker.


43

Figur 5-4: Giltiga, skymda och översegmenterade linjesegment.

5.1.3 Kovariansmatrisen för linjesegmentobjekt För att uppskatta osäkerheten för en mätning på ett linjesegment används osäkerheten för lasermätningarna som utgångspunkt. Tillverkaren av lasern delar in mätbruset i två delar [7]. Den ena delen är ett oberoende brus med väntevärdet noll. Den andra delen är ett brus som varierar beroende på vilket material objektet består av och vilket avstånd det befinner sig på. Detta brus kan följaktligen ha ett väntevärde som är skilt från noll, förutsatt att mätningarna utförs på samma objekt och på samma avstånd från lasern. Standardavvikelsen för bruset med väntevärde noll betecknas σf1 och standardavvikelsen för det andra bruset betecknas σf2. Först beräknas hur bruset med standardavvikelsen σf1 påverkar osäkerheten för linjesegmentsobjektet. Osäkerheten för linjesegmentsobjektets position i dess normalriktning och osäkerheten för linjesegmentsobjektets vinkelosäkerhet är beroende av varandra. Kovariansmatrisen för en mätning kan erhållas genom att invertera mätningens informationsmatris. Informationsmatrisen för de båda osäkerheterna ges enligt [24] av


44

−

−

=≈

−

−

∑∑=

21

2

21

21

21

21

122

1

11

f

i

f

ff

if

N

i ii

i

if N

NN

constσ

η

σ

η

σ

η

σ

σηηη

σ. ( 5-1)

Där ηi beskriver avståndet från punkten som representerar linjesegmentet till en mätpunkt projicerad på linjen, se figur 5-5 . Matrisen som ges av (5-1) är informationsmatrisen för linjesegmentsobjektets osäkerheter. Element (1,1) beskriver informationsmängden i mätningen om linjens position i dess normalriktning. Element (2,2) beskriver informationsmängden om linjens riktning. Övriga termer beskriver korrelationen mellan informationen om vinkeln och informationen om linjens läge i dess normalriktning. För att erhålla kovariansmatrisen inverteras informationsmatrisen.

Figur 5-5 : Variabler som används för att beräkna osäkerheten för en mätning på ett linjeobjekt, η markerar den punkt som representerar linjesegmentobjektet. Eftersom linjen representeras av en punkt är inte information om linjens position i normalriktningen nog, det behövs även information om punktens osäkerhet i linjens längdriktning. Om man antar att den verkliga slutpunkten är likformigt fördelad på två intervall som har längden 12 ηη − och 1−− NN ηη så är enligt [25] variansen för linjesegmentets startpunkt

12)( 2

122 ηησ

−=lb ( 5-2)

och variansen för dess slutpunkt


45

12)( 2

12 −−= NN

lbηη

σ . ( 5-3)

Den totala variansen i linjesegmentets längdriktning för punkten som representerar linjen är följaktligen

12)(

12)( 2

12

122 −−+

−= NN

lηηηη

σ . ( 5-4)

Alla tänkbara linjesegmentobjekt har inte skarpa kanter i ändarna, därför kan det vara befogat att lägga till en extra term på variansen i linjesegmentets längdriktning. Informationen om linjesegmentets läge i längdriktningen erhålls genom att invertera variansen för läget i längdriktningen. Informationsmatrisen för mätningen erhålls genom att matrisen i (5-1) utvidgas med informationen om linjesegmentets läge i längdriktningen. Kovariansmatrisen för linjesegment som en funktion av σf1 ges slutligen av

)(

0

010

01

)()(

21

21

21

2

21

21

21 γγ

σ

η

σ

η

σ

σ

η

σ

σ zM

N

N

invzMR

ff

l

ff

Tf

−

−

=

∑ ( 5-5)

där ( ) ( )( ) ( )

1000cossin0sincos

)(

−= aa

aaaM . ( 5-6)

Nedan beräknas hur bruset med standardavvikelsen σf2 påverkar osäkerheten för linjesegmentsobjektet. Eftersom σf2 är standardavvikelsen för ett brus som beror på vilket material linjesegmentet består av och på vilket avstånd från lasern det befinner sig förbättras inte mätningen av antalet mätpunkter. För att inte överskatta informationen om linjens riktning används bara en mätpunkt för att beräkna informationen om linjens riktning dock väljs den mätpunkt som innehåller mest information. För att inte överskatta korskorrelationen väljs den mätpunkt som ger upphov till lägst korskorrelation, denna mätpunkt betecknas ηmin. Kovariansmatrisen för mätbruset med standardavvikelsen σf2 ges av

)(

)max(0

010

01

)()(

22

221

22

min

2

22

min2

2

22 γγ

σ

ηη

σ

ησ

σ

η

σ

σ zMinvzMR

f

n

f

l

ff

Tf

−

−

= . ( 5-7)


46

Det kan påpekas att eftersom de flesta linjesegmentsobjekt består av samma material och befinner sig på ungefär samma avstånd från lasern är det i de flesta fall onödigt pessimistiskt att sätta informationen om linjens riktning till

22

221 ),,max(

f

n

σ

ηη

och korskorrelationerna till

22

min

fσ

η.

Kovariansmatrisen för mätbruset som används i kalmanfiltret har följande utseende

)(

00

010

001

)(

22

2

2

22

γ

γ

γ

σ

σ

σ

zM

Lkk

invzMR

f

Ll

f

T

+= , ( 5-8)

där kγ och kL är designparametrar. Bidraget från σf1 har försummats eftersom 22

2 ff σσ >> , enligt [7]. Korstermerna har också försummats eftersom väntevärdet 0≈η . En extraterm som ses som en designparameter har adderas till variansen i linjesegmentsobjektets längdriktning σl och informationen om linjesegmentsobjektets riktning sätts till

22

2

f

Lk

σγ .

L representerar linjesegmentets längd och kγ är en designparameter som bör väljas så att informationen om linjeobjektets riktning ligger inom intervallet

21

2

22

2

22

221 )max(

f

i

ff

n Lk

σ

η

σσ

ηη γ ∑<< . ( 5-9)

Det kan påpekas att denna brusmodell förutsätter att linjesegmentsobjekten är släta.

5.2 Linjeobjekt I mätdata från lasern finns det många linjer vars början eller slutpunkt är skymd, dessa linjer är inte giltiga linjesegmentsobjekt. För att utnyttja informationen i dessa vid positionering införs linjeobjekt. Ett linjeobjekt representeras med en punkt på linjen och riktningen på linjens normal. Anledningen till att linjer representeras med en punkt och en riktning i det jordfasta koordinatsystemet är att den framtagna mätekvationen ska gå att använda. Ett alternativt sätt att


47

representera linjeobjekt är att införa det vinkelräta avståndet mellan farkosten och linjen, benämnt ρ, och riktning för detta avstånd, benämnd γ, som tillstånd i kalmanfiltret. Mätekvationen skulle då få följande utseende

=

=

)ˆ()ˆ(

ˆˆ

ˆxhxh

zz

zγ

ρ

γ

ρ . ( 5-10)

Den punkt som har valts för att representerar en mätning på ett linjeobjekt är skärningspunkten mellan normalen till linjeobjektet och linjen som blidas av mätpunkterna. I figur 5-6 representerar z ett linjeobjekt transformerat till det robotrelativa koordinatsystemet och z representerar en mätning på detta linjeobjekt.

Figur 5-6 : Ett exempel då punkten som representerar mätningen på linjeobjektet ligger utanför linjesegmentet som bildas av mätpunkterna. Där ρz representerar det vinkelräta avståndet mellan robotens position och linjen

och γz representerar vinkeln mellan linjens normal och den positiva x-axeln för ett koordinatsystem som är fäst på roboten. Anledningen till att metoden som flyttar mätningen till objektet valdes är att med den metoden kan samma mätekvation användas för både linjeobjekt och linjesegmentsobjekt.


48

5.2.1 Extraktion av linjeobjekt ur laserdata Linjeobjekt kan extraheras på samma sätt som linjesegmentsobjekt. Ytterligare en metod att extrahera linjeobjekt har också använts. Denna metod kallas Houghtransformen.

5.2.2 Houghtransformen Med hjälp av Houghtransformen kan linjer extraheras ur data från en avståndsmätande laser. Enlig [26] är Houghtransformen för avståndsdata pålitligare än en Houghtransform utförd på en vanlig bild. Anledningen till att transformen är pålitligare för avståndsdata är att det sällan finns parallella linjer som ligger nära varandra i avståndsdata. I Houghrymden representeras en linje av det vinkelräta avståndet ρ från linjen till origo där lasern är placerad och vinkel γ som är vinkeln mellan laserns riktning och ρ, se figur 5-7. Avståndsdata från lasern representeras som ett antal punkter givna i polära koordinater (ri, φi).

Figur 5-7: En robot och en linje representerad med avståndet ρ och vinkeln γ. Den viktade versionen av Houghtransformen ges av

∑ −−=i

iiiih rgrwC ),,())cos((),( γϕργϕγρ . ( 5-11)

En mätning genomsöks efter linjer genom att beräkna (5-11) där alla ρ mellan 0 och ρmax med stegintervallet ∆ρ beräknas och alla γ från 0 till 2π med vinkelsteget ∆γ . Dessa värden lagras i en matris

.

)2,(),()0,(

)2,(),()0,()2,0(),0()0,0(

maxmaxmax

∆−∆

∆−∆∆∆∆∆−∆

=

γπργρρ

γπργρργπγ

CCC

CCCCCC

Cmatris (5-12)


49

I (5-11) är w en fönsterfunktion och g en viktfunktion. Argumentet i fönsterfunktionen är det kortaste avståndet mellan linjen (ρ, γ) och punkten (ri, φi). För att få en effektiv implementering används en rektangulär fönsterfunktion wh(x) med bredden 2a.

>≤

=axax

xwh ,0,1

)( ( 5-13)

Den viktfunktion som förespråkas i [26] är iiih rrg =),,( γϕ . ( 5-14)

En linje erhålls genom identifiering av det största värdet i Houghtransformen som ges av (5-12). Därefter tas de mätningar som är associerade med denna bort. Sedan körs Houghtransformen på de kvarvarande punkterna. Denna procedur upprepas tills alla värden större än ett förutbestämt minsta värde har identifierats. Linjerna från Houghtransformen förbättras sedan genom att en robust minstakvadratmetod används på de punkter som är associerade med en linje.

5.2.3 Inställning av Houghtransformen Då antalet vinkelsteg i Houghtransformen minskas är det större chans att missa en sökt vägg. Sträckan Lh i figur 5-8 är den sträcka som garanterat erhålls av en vägg då vinkelsteget är ∆γ, avståndsupplösningen är ∆ρ och ett rektangulärt fönster med vidden

2ρ∆

=a ( 5-15)

använts.

Figur 5-8: Lh är den längsta sträcka av en linje som garanterat upptäcks med Houghtransformen då avståndet mellan två linjer är ∆ρ, vinkelsteget mellan två linjer är ∆γ och ett rektangulärt fönster med bredden ∆ρ används. Genom geometriska samband kan denna längd beräknas

)2

sin( γ∆∆

=dLh . ( 5-16)

För små vinklar kan sin(∆γ/2) approximeras med ∆γ/2, detta ger att om ∆γ fördubblas så måste ∆ρ också fördubblas om Lh inte skall minska. Det minsta antalet vinkelsteg i Houghtransformen beror följaktligen på två saker, dels det


50

största värde som kan accepteras på ∆ρ, dels det minsta värde som kan accepteras på Lh . Då Houghtransformen använts i [26] har en vinkelupplösning på 120 steg använts och avståndsupplösningen har varit 2.5 cm. I [26] påpekas att beräkningstiden för Houghtransformen är proportionell mot antalet mätningar och antalet vinkelsteg, medan avståndsupplösningen inte påverkar beräkningstiden nämnvärt.

5.2.4 Kovariansmatrisen för linjeobjekt För att uppskatta osäkerheten för en mätning på ett linjeobjekt görs i princip på samma sätt som då osäkerheten för linjesegmentsobjekt beräknas. Om punkten som representerar mätningen ligger någonstans mellan första och sista mätpunkten på linjen används samma brusmodell som för linjesegment, enda skillnaden är att kL sätts till ett stort värde. Den enda gång då kL inte sätts till ett stort värde är då ett nytt linjeobjekt införs i kalmanfiltret. Detta görs för att undvika att punkten som representerar linjen glider längs linjen då mätuppdateringar i kalmanfiltret utförs. Kovariansmatrisen för mätbruset då punkten som representerar mätningen ligger bland mätningarna ges av

)(

00

010

001

)(

22

2

2

22

γ

γ

γ

σ

σ

σ

zM

Lkk

invzMR

f

Ll

f

T

+= . ( 5-17)

Om punkten som representerar mätningen ligger utanför linjesegmentet som bildas av mätpunkterna, se figur 5-6 , är inte korskorrelationerna noll i

)(

)max(0

010

01

)()(

22

221

22

min

2

22

min2

2

22 γγ

σηη

ση

σ

ση

σ

σ zMinvzMR

f

n

f

l

ff

Tf

−

−

= ( 5-18)

som fallet var för linjesegment. Kovariansmatrisen för mätbruset som används då punkten som representerar mätningen ligger utanför mätningarna ges av


51

)(

)(0

010

01

)()(

22

2

23

2

23

23

22 γ

γ

γ

σσ

σ

σσ

σ zM

LLkLk

L

invIzMR

f

c

f

c

Ll

f

c

f

Tf

+−

+

−

= , ( 5-19)

där kL och kγ är designparametrar. Genom geometriska samband som visas i figur 5-9 erhålls

2

)2

(23 L

LLc

ff

+= σσ , ( 5-20)

2221 )()max( cn LL +=ηη ( 5-21)

och cL=minη . ( 5-22)

Sträckan Lc är definierad så att den är positiv om punkten som representerar mätningen på linjeobjektet befinner sig till vänster om linjesegmentet och negativ om punkten befinner sig till höger om linjesegmentet som bildas av mätpunkterna. Lc är noll om punkten som definierar linjeobjektet befinner sig inom linjesegmentet som bildas av mätpunkterna.

Figur 5-9: Illustration av hur osäkerhet i linjens normalriktning ökar om punkten som representerar mätningen på linjen ligger utanför linjesegmentet som bildas av mätpunkterna.


52

5.2.5 Segmentering av linjeobjekt Ett sätt att segmenterat linjeobjekt är att enbart försöka associera mätningar på linjeobjekt med de objekt som finns inom en given radie rmax från mätningen. På detta sätt segmenteras linjer med längden rmax eftersom mätningar som inte associeras läggs in som nya objekt. Denna radie blir följaktligen en designparameter för längden på linjeobjekt.

Experiment . .

53

6 Experiment I detta avsnitt presenteras testresultat från positionering med både linjeobjekt och linjesegmentobjekt. Testerna har utförts i inomhusmiljö och mätningar har utförts med en samplingshastighet på ca 3 Hz. Positioneringstesten är ej utförda i realtid utan har utförts i efterhand på de insamlade mätningarna.

6.1 Positionering med linjesegmentsobjekt Testet av linjesegmentobjekt är utfört i en stor lokal där många typer av föremål är uppställda. I figur 6-1 visas ett foto av den miljö där positionering med linjesegmentsobjekt har testats. Eftersom miljön innehåller många plana ytor är den väl lämpad att användas som testmiljö för test av positionering med linjesegmentsobjekt.

Figur 6-1: Miljön där experiment av positionering med linjesegmentsobjekt utförs. Vid testet rör sig farkosten en sträcka av totalt ca 20m. Den väg farkosten färdas under experimentet visas i figur 6-2. Farkosten börjar med att färdas framåt ett stycke därefter backar den tillbaka en bit denna procedur upprepas några gånger. Därefter backar farkosten till sin slutliga position.

Experiment . .

54

Figur 6-2: Farkostens färdväg under experimentet. Då positioneringen startas saknas förhandsinformation om omgivningen och dess landmärken. Algoritmen för att extrahera linjesegmentsobjekt används på data från den svepande avståndsmätande lasern för att extrahera landmärken. Då nya landmärken upptäcks läggs de in som tillstånd i kalmanfiltret. Efterföljande mätningar på samma landmärke används för att uppdatera farkostens position. Det här är ett exempel på SLAM eftersom ingen förhandsinformation om omgivningen erhålls på förhand. I figur 6-3 visas alla lasermätningar utlagda med hjälp av farkostens position då den beräknas med positioneringsalgoritmen. Som en jämförelse har samma mätningar lagts ut enbart med hjälp av farkostens dödräkning. Detta visas i figur 6-4.

Experiment . .

55

Figur 6-3: Lasermätningar utplacerade med farkostens position uträknad av positioneringsalgoritmen. Linjen visar farkostens färdväg.

Figur 6-4: Lasermätningar utplacerade med hjälp av farkostens dödräkning. Linjen visar den väg farkosten färdats.

Experiment . .

56

Vid jämförelse mellan figur 6-3 och figur 6-4 ses en tydlig förändring i form av att lasermätningarna är mer samlade kring objekt i figur 6-3. Detta beror på att farkosten kunnat utföra en bättre positionering. För att tydliggöra förbättringen förstoras ett område innefattande ett hörn. Ett hörn är ett bra område eftersom det bekräftar att farkosten haft god kännedom om sin position i alla riktningar. Hörnet med position x=-6 och y=-0.5 är synligt under största delen av datainsamlingen. En förstoring av nämnda hörn visas i figur 6-5 och figur 6-6

Figur 6-5: Lasermätningar utplacerade med hjälp av dödräkning.

Experiment . .

57

Figur 6-6: Lasermätningar utplacerade med hjälp av kalmanfiltret. Det kan påpekas att en stor skiva var lutad mot den ena väggen då experimentet utfördes. Om figur 6-5 studeras ses att spridningen på mätningarna är ca 20 cm då data är utplacerat med hjälp av dödräkning. Från figur 6-6 erhålls att spridningen är ca 5 cm. Detta bör betraktas som ett bra positioneringsresultat med tanke på att standardavvikelsen för lasermätningarna är i storleksordningen centimeter. I figur 6-7 visas de extraherade och använda landmärkena tillsammans med några lasersvep gjorda i labbet.

Experiment . .

58

Figur 6-7: Linjesegmentsobjekten som använts för positionering. Om figur 6-7 betraktas ses att positioneringsalgoritmen har extraherat verkliga plana ytor som är helt synliga och inte skymda av några andra objekt.

6.2 Positionering med linjeobjekt Testet av linjeobjekt utförs genom att farkosten som bär sensorplattformen färdas 60 meter i en korridor, svänger in i en sidokorridor, vänder och färdas samma väg tillbaka. Att börja och starta på samma position är en bra test av positioneringsalgoritmen eftersom det är lätt att se om estimerad startpunkt och slutpunkt sammanfaller. För att sätta kalmanfiltret på prov adderas ett fel på två procent till den ena hjulradien. Då positioneringen startas saknas förhandsinformation om omgivningen och dess landmärken. Algoritmen för att extrahera linjeobjekt används på data från den svepande avståndsmätande lasern för att extrahera landmärken. Då nya landmärken upptäcks läggs de in som tillstånd i kalmanfiltret. Efterföljande mätningar på samma landmärke används för att uppdatera farkostens position. Det här är ett exempel på SLAM eftersom ingen förhandsinformation om omgivningen erhålls på förhand. I figur 6-8 visas laserdata utplacerade med hjälp av farkostens dödräkning och i figur 6-9 visas laserdata utplacerad med positioneringsalgoritmen.

Experiment . .

59

Figur 6-8: Laserdata utplacerade med hjälp av dödräkning. Ett fel på två procent är adderat till den ena hjulradien. Farkosten startar i origo och i verkligheten slutar färden också i origo men på grund av det adderade felet hamnar slutpunkten ca 100 m fel.

Figur 6-9 : Laserdata utplacerade med hjälp av kalmanfiltret. Ett fel på två procent är adderat till den ena hjulradien. Linjeobjekten segmenteras då de överstiger 15 m.

Experiment . .

60

Om figur 6-8 studeras ses att det adderade felet gör att slutpunkten som i verkligheten sammanfaller med startpunkten hamnar ca hundra meter från startpunkten. I figur 6-9 ses att startpunkten sammanfaller med slutpunkten, samt att kalmanfiltret lyckats göra korridorens väggar räta. Att startpunkt och slutpunkt sammanfaller då positioneringsalgoritmen används visar att den är kapabel att utföra robust positionering.

Slutsatser och fortsatt arbete . .

61

7 Slutsatser och fortsatt arbete I detta kapitel presenteras konklusioner och förslag till fortsatt arbete. Slutsatserna är dragna utifrån tänkbara tillämpningar och de utförda experimenten.

7.1 Slutsatser Syftet med detta examensarbete var att driftsätta en sensorplattform och utföra positionering med verklig data. Positioneringen är framförallt tänkt att kunna användas då GPS-signaler ej är tillgängliga. Tillfällen då GPS signaler inte är tillgängliga är t.ex. i närheten av stora byggnader eller i inomhusmiljö. Dessa miljöer innehåller många plana ytor som skulle gå att använda som linjeobjekt eller linjesegmentsobjekt. Utförda experiment visar att positioneringsalgoritmerna kan utföra positionering med god säkerhet i sådana miljöer. Det finns andra miljöer där GPS signalerna kan vara skymda, t.ex. i närheten av stora träd. En sådan miljö saknar helt plana ytor, följaktligen skulle inga landmärken kunna extraheras av de algoritmer som används. Positioneringen skulle i en sådan miljö endast utföras av dödräkningen. Experimenten visar att positionsfelet snabbt ökar vid en förändring av hjulradien, om positioneringen endast utförs med hjälp av dödräkning. En förändring av en hjulradie kan i verkligheten tänkas ske genom att lufttrycket ändras i ett av hjulen eller av att stenar eller snö fastnar på ett av hjulen. Om underlaget är ojämnt eller har låg friktion skulle positionering utförd endast med hjälp av dödräkning antagligen snabbt bli felaktig eftersom hjulen antagligen skulle glida istället för att rulla i vissa situationer. För att utföra robust positionering i miljöer som saknar plana ytor skulle nya typer av objekt behöva införas t.ex. cirkar. Ett annat sätt att utföra positionering i miljöer där bra landmärken saknas vore att flera robotar samarbetade och övervakade varandras rörelser.

7.2 Förslag till fortsatt arbete Under arbetets gång har flera områden som skulle vara intressanta att undersöka ytterligare påträffats. Nedan ges förslag på några av de områden som skulle vara intressant att undersöka vidare. Nya typer av objekt. De objekt som används för positionering är linjer och linjesegment. Andra typer av objekt som också kan extraheras ur laserdata är t ex cirklar. Om flera robotar samverkar så att de tillsammans ser alla sidor på ett objekt skulle det vara möjligt utföra positionering med objekt av godtycklig form. Införa flera robotar i filtret. Filtret är förberett för att hantera flera robotar. Ett problem som kvarstår är att hantera att de olika robotarnas mätningar sker vid olika tidpunkter. I [4] har detta problem behandlats och två robotar införts i kalmanfiltret.

Slutsatser och fortsatt arbete . .

62

Skatta de parametrar som kalibrerats. I rörelsemodellen för roboten ingår hjulradierna och axelavståndet som parametrar. Dessa parametrar kan ge upphov till systematiska fel om de inte kalibrerats ordentligt eller om de ändras efter kalibreringen. En hjulradie kan t.ex. ändras om roboten lastas tyngre eller om en sten fastnar på ett hjul. För att undvika systematiska fel skulle hjulradierna och axelavståndet kunna införas som tillstånd i kalmanfiltret. Vidare är lasern placerad mitt över hjulaxel om denna placering inte är exakt uppstår också systematiska fel. För att undvika dessa och tillåta en annan placering av lasern kan laserns placering på roboten införas som ett tillstånd i kalmanfiltret. Ny brusmodell för roboten. I den brusmodell som används saknas möjligheten att modellera att roboten glider i sidled. Brusmodellen är också ganska komplicerad vilket inte ger en bra intuition om vad som händer då en av parametrarna ändras. En enklare brusmodell som ger möjlighet att modellera glidning i sidled vore en önskvärd förbättring. I [4] används en annan brusmodell som inte är lika komplicerad. Bättre associeringsalgoritm. Associeringen är en viktig faktor för hur bra kalmanfiltret fungerar eftersom en felassociering kan leda till stora fel i positioneringen. Den associeringsalgoritm som används är väldigt enkel. En förbättring vore att införa en mer avancerad associeringsalgoritm. Reducera antalet objekt i kalmanfiltret. Om roboten rör sig fritt blir antalet objekt förr eller senare så stort att beräkningarna inte kan utföras inom önskad tidsram. För att reducera antalet objekt i kalmanfiltret är en lösning att bara uppdatera objekt som befinner sig nära roboten. En annan lösning kan vara att ha olika kalmanfilter för olika områden. Någon metod att reducera antalet objekt är antagligen nödvändig för de flesta verkliga applikationer. Nya typer av sensorer. Förslag på sensorer som sensorplattformen kan utökas med är en sonar, en IMU (Inertial measurement unit) och en kompass.

.

63

8 Referenser

[1] I. S. Roumeliotis, A. B. Bekey, A distributed Kalman filter approach to localization of groups of mobile robots, IEEE conference on Robotics & Automation in San Francisco, 2000

[2] I. Rekleitis, R. Sim, G. Dudek, E. Milios, Collaborative exploration for the construction of visual maps, IEEE/RSJ conference on Intelligent Robots and Systems, 2001

[3] L. Andersson, J. Nygårds, On Sensor Fusion Between a Pair of Heterogeneous Robots, to be presented at FUSION, 2003

[4] P. Holmberg, Sensor Fusion with Coordinated Mobile Robots, Master Thesis to be presented, LiTH-ISY-EX-3387-2003

[5] J.Göransson. Examensarbete utfört åt Robotik/Autonoma Mekaniska System vid IKP LiTH. Master Thesis to be presented.

[6] U. Larsson, On Robot Control with Feedback from Optronic Sensors, Doctoral thesis, LuTH, 1999

[7] TECHNICAL DESCRIPTION LMS 200 / LMS 211 /LMS 220 / LMS 221 / LMS 291 Laser Measurement Systems. 8 008 970/01-2002 . WU/SM. Printed in Germany (01-02). PreE3305c

[8] R. Krten, Getting Started with QNX Neutrino 2 A Guide for Realtime Programmers, 2001 PARSE Software Devices, ISBN 0-9682501-1-4

[9] B. P. Gerkey, R. T. Vaughan, A. Howard. Player version 1.2.3 User Manual. USC Robotics Laboratory University of Southern California, USA, http://playerstage.sourceforge.net

[10] P. Rydesäter TCP/UDP/IP Toolbox 2.0.2. http://www.mathworks.com

[11] R. Kalman, (1960),’A new approach to linear filtering and prediction Problems’, Transactions of the ASMS 82, 35-45

[12] P. Jensfelt, Approaches to Mobile Robot Localization in Indoor Environments.TRITA-S3-REG-0102,ISSN 1404-2150, ISBN 91-7283-135-9

[13] B. D. O. Anderson, & J. B. Moor, (1979), Optimal Filtering, Prentice-Hall, Englewood Cliffs, N. J.

[14] A. H. Jazwinski, (1970), Stochastic Processes and Filtering Theory, Vol. 64 of Mathematics in Science and Engineering, Academic Press.

[15] J. L Crowley, (1995), Mathematical foundations of navigation and perception for an autonomous mobile robot’. Workshop on Reasoning with Uncertainty in Robotics.

[16] H. Durrant-Whyte, (1994), ’Where am I’, Industrial Robot 21,11-16 [17] T. Kailath, A. Sayed, &B. Hassibi, (2000), Linear Estimation, Prentice

Hall. ISBN: 0-13-022464-2 [18] Welch, G. & Bishop,G. (2001), An introduction to the kalman filter,

Technical Report TR 95-041, Department of Computer Science, University of North Carolina at Chapel Hill, Chapel Hill, NC 27599-3175.

.

64

[19] R. Karlsson. Simulation Based Methods for Target Tracking.

LiU-Tek-LIC-2002:03, Printed by UniTryck, Linköping, Sweden 2002, ISBN 91-7373-267-2

[20] Y. Bar-Shalom, and T. Fortman, (1988). Tracking and Data Association, volume 179 of Mathematics in Science and Engineering. Academic Press.

[21] I. J. Cox, G.T. Wilfong, Autonomous Robot Vehicles, 1990 Springer Verlag,

[22] A. Persson L. Böiers. Analys i en variabel. Studentlitteratur 1990. ISBN 91-44-31341-1

[23] L. Zhang and B. K. Ghosh. Line Segment Based Map Building and Localization Using 2D Laser Rangefinder. Proceedings of the 2000 IEEE International Conference on Robotics & Automation San Francisco, CA April 2000.

[24] J. Nygårds, Å. Wernersson, On Covariances for Fusing Laser Rangers and Vision with Sensors Onboard a Moving Robot, Proceedings IEEE/RSJ conference on Intelligent Robots and Systems, 1998

[25] G. Blom. Sannolikhetsteori och statistikteori med tillämpningar. Studentlitteratur, Lund, Sverige, fjärde upplagan, 2000, ISBN 91-44-03594-2

[26] J. Forsberg, U.Larsson and Åke Wernersson. Mobile Robot Navigation Using the Range-Weighted Hough Transform. IEEE Robotics & Automation Magazine special issue on Mobile Robots, pp. 18-26,March 1995.

65

På svenska Detta dokument hålls tillgängligt på Internet – eller dess framtida ersättare – under en längre tid från publiceringsdatum under förutsättning att inga extra-ordinära omständigheter uppstår.

Tillgång till dokumentet innebär tillstånd för var och en att läsa, ladda ner, skriva ut enstaka kopior för enskilt bruk och att använda det oförändrat för ickekommersiell forskning och för undervisning. Överföring av upphovsrätten vid en senare tidpunkt kan inte upphäva detta tillstånd. All annan användning av dokumentet kräver upphovsmannens medgivande. För att garantera äktheten, säkerheten och tillgängligheten finns det lösningar av teknisk och administrativ art.

Upphovsmannens ideella rätt innefattar rätt att bli nämnd som upphovsman i den omfattning som god sed kräver vid användning av dokumentet på ovan beskrivna sätt samt skydd mot att dokumentet ändras eller presenteras i sådan form eller i sådant sammanhang som är kränkande för upphovsmannens litterära eller konstnärliga anseende eller egenart.

För ytterligare information om Linköping University Electronic Press se förlagets hemsida http://www.ep.liu.se/ In English The publishers will keep this document online on the Internet - or its possible replacement - for a considerable time from the date of publication barring exceptional circumstances.

The online availability of the document implies a permanent permission for anyone to read, to download, to print out single copies for your own use and to use it unchanged for any non-commercial research and educational purpose. Subsequent transfers of copyright cannot revoke this permission. All other uses of the document are conditional on the consent of the copyright owner. The publisher has taken technical and administrative measures to assure authenticity, security and accessibility.

According to intellectual property law the author has the right to be mentioned when his/her work is accessed as described above and to be protected against infringement.

For additional information about the Linköping University Electronic Press and its procedures for publication and for assurance of document integrity, please refer to its WWW home page: http://www.ep.liu.se/ © [Jonas Kjellander]

Positionering och kartuppbyggnad med …19081/FULLTEXT01.pdf · Det finns flera olika metoder för...

Documents

Transcript of Positionering och kartuppbyggnad med …19081/FULLTEXT01.pdf · Det finns flera olika metoder för...