Tools and Algorithms for Mobile Robot Navigation with Uncertain Localization

hesis y
Kristopher L. Kriechbaum

sn rtil pulllment of the equirements for the hegree of hotor of hilosophy

gliforni snstitute of ehnology sdenD gliforni PHHT @hefended wy PQD PHHTA

ii

PHHT uristopher vF uriehum ell ights eserved

iii

iv

Acknowledgements
s wish to thnk my dviserD toel furdikD for his guidneD supportD nd onstnt ptieneF s would like to thnk tergios oumeliotis for tehing me ll s know out ulmn ltersF wy thnks to the rootis group for their help nd feedkF s would like to thnk ll the memers of the glteh hee qrnd ghllenge tem for providing me with suh unique experieneF wy good friends nd oemtes vrs gremen nd tim dford provided mny worthE while disussionsD s well s muh needed distrtionsF wny thnks to wihel olfD xiols rudsonD elexndros nidisD wohmed ilExggrD toseph ulmoD nd imothy ghung for the ttles they providedF s lso wnt to thnk the eero nd y soer tems for giving me n exuse not to work on the weekendsD nd the glteh glu oer tem for giving me n exuse not to work eveningsF s would like to thnk my fmily for their love nd supportD nd ypheli for the unonE ditionl love tht only dog n giveF wost of llD s would like to thnk my wifeD wriD for her ontinuous enourgement throughout my grdute reerF

Abstract
he ility for moile root to lolize itself is si requirement for relile long rnge utonomous nvigtionF his thesis introdues new tools nd lgorithms to id in root loliztion nd nvigtionF s introdue new rnge sn mthing method tht inorportes relisti sensor noise modelsF his method n e thought of s n improved form of odometryF esults show n order of mgnitude of improvement over typil moile root odometryF sn dditionD s hve reted new sensorEsed plnning lgorithm where the root follows the lolly optiml pth to the gol without exeptionD regrdless of whether or not the pth moves towrds or temporrily wy from the golF he ost of pth is dened s the pth lengthF his new lgorithmD whih s ll yptimEfugD4 is omplete nd orretF pinllyD s developed new onEline motion plnning proedure tht determines pth to gol tht optimlly llows the root to lolize itself t the golF his lgorithm is lled nertin fugF4 sn prtiulrD the ovrine of the root9s pose estimte t the gol is minimizedF his hrteristi inreses the likelihood tht the root will tully e le to reh the desired golD even when unertinty orrupts its loliztion during movement long the pthF he root9s pth is hosen so tht it n use known fetures in the environment to improve its loliztionF his thesis is rst step towrds ringing the tools of moile root loliztion nd mpping together with ides from sensorEsed motion plnningF

vi

Contents

Acknowledgements Abstract 1 Introduction

iv v 1

IFI IFP IFQ IFR

wotivtion F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F eview of rior ork F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F gontriutions of this hesis F F F F F F F F F F F F F F F F F F F F F F F F F F F hesis yutline F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F

I P S T

2 Background

PFI ngent fug F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F PFP ulmn piltering F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F PFPFI he fsis F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F PFPFP ulmn pilters nd ooti voliztion F F F F F F F F F F F F F F F PFPFPFI voliztion of oint oot sing vndmrks F F F F F F QFI sntrodution F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F QFP he eighted nge ensor wthing rolem F F F F F F F F F F F F F F F F QFPFI he wesurement wodel F F F F F F F F F F F F F F F F F F F F F F F F QFPFP e qenerl govrine wodel F F F F F F F F F F F F F F F F F F F F F F QFPFQ hisplement istimtion vi wximum vikelihood F F F F F F F F F F QFPFR he elgorithm nd sts snitil gonditions F F F F F F F F F F F F F F F QFPFS govrine of the hisplement istimtion irror F F F F F F F F F F F QFQ n wthing irrorGxoise wodels F F F F F F F F F F F F F F F F F F F F F F F

V II IP IS IS PI PS PS PU PV QH QI QQ

3 Weighted Scan Matching

21

QFR QFS QFT

QFU RFI RFP RFQ RFR RFS SFI SFP SFQ SFR SFS

vii QFQFI wesurement roess xoise F F F F F F F F F F F F F F F F F F F F F F F QFQFP gorrespondene irror F F F F F F F F F F F F F F F F F F F F F F F F F F QFQFQ wesurement fis iets F F F F F F F F F F F F F F F F F F F F F F F F eletion of oint gorrespondenes F F F F F F F F F F F F F F F F F F F F F F F istimting the snidene engle F F F F F F F F F F F F F F F F F F F F F F F F F ixperiments F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F QFTFI oustness nd eury gomprisons F F F F F F F F F F F F F F F F QFTFP wultiEtep uns F F F F F F F F F F F F F F F F F F F F F F F F F F F F F QFTFQ gomprison of gomputtionl hemnds F F F F F F F F F F F F F F F F QFTFR ixperiments with fis gompenstion F F F F F F F F F F F F F F F F F gonlusions F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F etup nd henitions F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F yptimEfug yverview F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F hortest th roperties F F F F F F F F F F F F F F F F F F F F F F F F F F F F F roof of gompleteness F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F gonlusion F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F

QQ QR QU QV QW RH RH RU SH SI SP
54

4 Optim-Bug

SR ST SW TH UI UP US UT UW VH VP VQ VR VS

5 Path Optimization

he voliztion prmework F F F F F F F F F F F F F F F F F F F F F F F F F F gost puntion F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F rtil yptimiztion epproh F F F F F F F F F F F F F F F F F F F F F F F F snitil gondition qenertion F F F F F F F F F F F F F F F F F F F F F F F F F F F esults F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F SFSFI oot hrives st the qol F F F F F F F F F F F F F F F F F F F F F F F SFSFP oot pollows the vndmrks F F F F F F F F F F F F F F F F F F F F F F SFSFQ ixmples with ystles F F F F F F F F F F F F F F F F F F F F F F F F SFT ummry nd hisussion F F F F F F F F F F F F F F F F F F F F F F F F F F F F

72

6 Uncertain Bug

TFI etup nd henitions F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F VW TFP wotivtion nd fkground F F F F F F F F F F F F F F F F F F F F F F F F F F F WI

88

TFQ TFR TFS TFT

viii he nertin fug elgorithm F F F F F F F F F F F F F F F F F F F F F F F F F F TFQFI iets of nertinty F F F F F F F F F F F F F F F F F F F F F F F F F F nertin fug elgorithm roperties F F F F F F F F F F F F F F F F F F F F F F imultion esults F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F hisussion F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F

WP WR WT IHH IHP
105

7 Conclusion

UFI ummry of gontriutions F F F F F F F F F F F F F F F F F F F F F F F F F F F F IHS UFP puture hiretions F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F IHT eFI eighted rnsltion olution F F F F F F F F F F F F F F F F F F F F F F F F F IHU eFP eighted ottion olution F F F F F F F F F F F F F F F F F F F F F F F F F F F IHU eFQ govrine istimtion F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F IIH fFI ystle gonstrints F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F IIR fFIFI he gonstrint iqutions F F F F F F F F F F F F F F F F F F F F F F F F IIT
107

A Weighted Scan Matching Derivations

B Implementation Details

114

C Shortest Path Properties D Bounded Uncertainty Freespace

119 124

hFI qol ynly F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F IPR hFP qol nd yne vndmrk F F F F F F F F F F F F F F F F F F F F F F F F F F F F F IPT hFQ qol nd wultiple vndmrks F F F F F F F F F F F F F F F F F F F F F F F F F F IPV
129

Bibliography

ix

List of Figures
PFI PFP PFQ PFR PFS PFT PFU QFI QFP QFQ QFR QFS QFT QFU QFV he lol tngent grphD with the optionl gol nodeD Tg D ddedF F F F F F F F e sitution where the root would swith into oundry following modeF F F en exmple ngent fug sequene where the root thinks it is t the golD ut the error is lrgeF F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F etup of ulmn lter for moile root loliztionF F F F F F F F F F F F F F e threeEstep propgtion exmpleF his shows how the estimte error ontinE ues to growF F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F e singleEstep propgtion exmple showing multiple trilsF F F F F F F F F F F ixmple dt illustrting using lndmrk for loliztionF F F F F F F F F F F qeometry of the rnge sensing proessF he root quires dense rnge sns in poses i nd j F he irles represent root positionD while the xEy xes denote the root9s ody xed referene frmesF F F F F F F F F F F F F F F F F F F F F F epresenttion of the unertinty of seleted rnge sn pointsF F F F F F F F F eA ixperiments with initil displement perturtions etween sns tken t single poseF fA gloseup of root pose with resultsF F F F F F F F F F F F F eA ixperiments with initil displement perturtions etween sns tken t dierent posesF fA gloseup of pose P with resultsF F F F F F F F F F F F F F F eA ixperiments with initil displement perturtions in nonEstti enviE ronmentF fA gloseup of pose P with resultsF F F F F F F F F F F F F F F F F F F F eA ixperiments with initil displement perturtions in hllwy environE mentF fA gloseup of pose P with resultsF F F F F F F F F F F F F F F F F F F F F F eA e IHWEpose QPFVEmeter loop pthF fA gloseup of nl pth posesD shown the ovrine estimtes of the weighted nd unweighted lgorithmsF F F F F F eA VQEpose PRFPEmeter loopF fA gloseup of nl loop posesF F F F F F F F F F F W IH IP IS IW IW PH

PP PR RP RR RS RT RV RW

QFW RFI RFP RFQ RFR RFS

x he is t funtionF F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F SI he current visibility setsD v@xk AD shown t eh lotion of fourEstep pthF et x D ostle y is blocking obstacleF F F F F F F F F F F F F F F F F F F F F F he total visibility setD VtotD for the sme fourEstep hypothetil exmple of pigure RFIF F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F e hypothetil exmple showing ostle endpointsD tngent pointsD nd their ssoited golEtngent linesF F F F F F F F F F F F F F F F F F F F F F F F F F F F sllustrtion for vemm RFI showing tht the totl visiility setD Vtot grows t eh stepF F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F et every step while nvigting round loking ostleD the root lwys sees new segment of the ostle oundry tht is ontiguous with previE ously known segmentF st my see dditionl oundry segments tht re not ontiguousF F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F e hypothetil sequene of steps showing how yptimEfug n osillte k nd forth long loking ostleF his lso illustrtes how ontinuous piee of the oundry segment is mpped outF F F F F F F F F F F F F F F F F F e hypothetil exmple where the root is loked y the sme ostle twieF ortions of the ostle in the root9s mp re denoted y drker olorF F F
2 2

SS ST TH TP

RFT RFU SFI

TS TU TV

SFP SFQ SFR SFS

he environment setup ssumed for the pth plnning proessF he root strts t lotion xR nd is instruted to trvel to xg F L D L D L D nd L re lndmrks tht the root hs knowledge ofF he irles re ostles tht the root must voidF F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F mple mesurement vrine prole s funtion of rngeF  a :ID a IHD nd rmax a QF xote tht the yExis sle is logrithmiF F F F F F F F F F F F F ixmple where the root initilly drives pst the gol in order to use lndE mrkF por simpliityD the sensing rnge is plotted out the lndmrk E if the root moves inside of tht irleD it is lose enough to see the lndmrkF F F F e multiEstep pth where the root signintly devites from the shortest pth in order to use the lndmrksF F F F F F F F F F F F F F F F F F F F F F F F F F F en simple exmple with ostlesF F F F F F F F F F F F F F F F F F F F F F F F F
1 2 3 4

UQ VI VQ VR VS

SFT TFI TFP TFQ TFR TFS TFT fFI fFP gFI

xi he sme lndmrk ongurtion s tht of pigure SFRF ystles were pled to lok the originl optiml pthF F F F F F F F F F F F F F F F F F F F F F F F F VT etup ssumed for the nertin fug lgorithmF he root does not know the position of ostles in the environmentF he root9s sensor hs nite sensing rngeD rF F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F WH rypothetil exmple where the root hooses pth tht uses it to step into n unertinty ostleF F F F F F F F F F F F F F F F F F F F F F F F F F F F WS pigure illustrting n extr termintion ondition for nertin fugF he next root pth is shown y the dshed lineF he root will follow the pth to the point leled x F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F WV e simultion of nertin fugF he ngent fug pth is shown s the pink dshed lineF pinl ost t the gol is QW7 of the ost of the ngent fug pthF IHI he plnned pth for steps W nd IHF xote tht when the root sees the ostleD it reEplns the pthD tking into ount the new informtion out the worldF F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F IHP e simultion of nertin fugF he ngent fug pth is shown s the pink dshed lineF pinl ost t the gol is QP7 of the ost of the ngent fug pthF IHQ
2

e portion of the pth where the root psses y n ostleF F F F F F F F F F IIS etup of single line nd irleF F F F F F F F F F F F F F F F F F F F F F F F F F IIS e xed endpoint vritionF F F F F F F F F F F F F F F F F F F F F F F F F F F F F IPH

hFI wximum size of ovrine ellipse to t inside irle with rdius of F F F F F IPS hFP et of points from whih the gol n e rehed with max < F he set is the drkened oxF F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F IPT hFQ e ound on the set of points from whih the gol ould e rehed with max < FIPU hFR nion of the multiple setsF F F F F F F F F F F F F F F F F F F F F F F F F F F F F IPV

xii

List of Tables
QFI QFP osition nd orienttion error vlues for trils with no initil perturtionsF F RI oustness nd ury omprison sttistis for trils with initil perturE tionsF F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F F RI

Chapter 1
Introduction
1.1 Motivation

he ility for moile root to lolize itself is si requirement for relile long rnge utonomous nvigtionF his thesis introdues new tools nd lgorithms to id in root loliztion nd nvigtionF s introdue new rnge sn mthing method tht inorportes relisti sensor noise modelsF his method n e thought of s n improved form of odometryF esults show n order of mgnitude of improvement over typil moile root odometryF sn dditionD s hve reted new sensorEsed plnning lgorithm where the root follows the lolly optiml pth to the gol without exeptionD regrdless of whether or not the pth moves towrds or temporrily wy from the golF he ost of pth is dened s the pth lengthF his new lgorithmD whih s ll yptimEfugD4 is omplete nd orretF ome of the ides nd issues from yptimEfug re used to ssist in the disussion of the se where the root does not hve perfet positionl knowledgeF pinllyD s developed new onEline motion plnning proedure tht determines pth to gol tht optimlly llows the root to lolize itself t the golF his lgorithm is lled nertin fugF4 sn prtiulrD the ovrine of the root9s pose estimte t the gol is minimizedF his hrteristi inreses the likelihood tht the root will tully e le to reh the desired golD even when unertinty orrupts its loliztion during movement long the pthF s ssume tht the root hs numer of @possily unertinA lndmrks ville to id in its loliztion proessF he root9s pth is hosen to exploit these lndmrks nd use them for etter loliztionF s lso ssume tht the root hs noisy odometryF reneD the lgorithm eetively nds lne etween longer pths tht pss lose to lndmrks @therey inresingly loliztion ilityA nd shorter pths

P tht introdue less odometry errorF his method n lso inlude ny other loliztion ids sed on either proprioeptive @eFgFD inertil nvigtion unitA or exteroeptive @eFgFD lser sn mthing lgorithms RPA whose opertion n e modeled in ulmn lter frmeworkF sn the simultions presented in this thesisD s fous stritly on the omintion of lndmrks nd onEord odometryF wo highly desirle fetures of ny plnner re ompleteness nd orretnessF e pth is correct if it lies wholly within the freespe @iFeFD the pth does not interset ny ostlesAF gompleteness mens tht the plnner will generte pth to the gol if one existsD nd will terminte in nite mount of timeF issentilly ll of the omplete nd orret sensorEsed motion plnning lgorithms ssume tht the root hs perfet knowledge of its lotion t ll times @eFgFD IID PHD QVAF yver very short distnesD this ssumption my not e so dD nd the root will likely get lose enough to the gol to e onsidered suessfulF xonethelessD even in modertely sized environmentsD the loliztion error my grow to the point tht the root is lost for ll intents nd purposesF he eld of simultneous loliztion nd mpping @vewA provides mny useful tools to help keep the root9s error lowF roweverD vew lgorithms do not fully ddress the issue of nvigtionF st is the disonnet etween the nvigtion prolem nd the loliztion nd mpping prolem tht s hope to ridge with this workF wy pth plnning method tkes into ount the rih ody of literture nd umulted experiene with using the ulmn lter for loliztionF roweverD prior work in loliztion nd mpping hs not fully ddressed the prolem of how to pln orret pth to gol while tking into ount possile loliztion unertintyF snstedD the primry fous hs een on inorporting newly disovered lndmrks into mpD nd lolizing the root y using mesurements of urrently known lndmrks QID SHD SSF
1.2 Review of Prior Work

ome of the erliest omplete nd orret sensorEsed motion plnning lgorithms re the fug lgorithms y vumelsky nd tepnov QVF he fug lgorithms ssume nothing more thn point root with sensor tht n detet whether or not the root is touhing n ostleF hey prove tht the root will reh the gol fter nite mount of time if the gol is rehleF hese lgorithms re the origin of lter fug4Elike plnnersF

Q umonD imonD nd ivlin extended the fug lgorithms to the se of sensor with nite rnge nd QTH eld of view in ngent fug PHF ngent fug produes locally optimal pthsF he pths re lolly optiml in tht they re the optiml pths given the root9s limited knowledge of the environmentF vuh modied the lgorithm further to the se of sensor with more limited eld of view in edge fug PTF ell of the fugEstyle lgorithms re eient with respet to memory requirementsF roweverD they ll require tht the root hve perfet position knowledge t ll timesF odmp methodsD suh s ghoset9s rqq IID tke dierent pproh to the proE lemF he rodmp ptures interesting topologil fetures of the environmentD suh s the onnetivity of two djent roomsF he rodmp n e more eient thn other pprohesD suh s uilding up mp in grtesin oordintesF sn ll of these methods the proof of onvergene of the root to the gol requires perfet ded rekoning for the rootF here re numerous methods ville to keep the root9s position estimte urteF sn some sesD they mke the perfet dedErekoning ssumption trtleF wthing PEh rnge sns from seprte root positions hs een proposed y qonzlez nd qutierrez IU nd vu nd wilios QTD QUF heir sn mthing lgorithms ll ssume tht the physil sensor returns perfet rnge mesurementsF hey lso ssume tht the rnge sns t dierent root positions smple the environment oundry t extly the sme pointF eny method of improved odometry will still suer from the prolem of growing position estimte error over timeF o redue the estimte errorD methods using mesurements of externl fetures must e usedF irly works using ulmn lters IW for root loliztion were proposed y woutrlier nd ghtil RHD gox IRD nd mith et lF RWD SHF yne ssumes tht the root is given the lotion of numer of lndmrks or fetures in the environment tht the root n reognizeF ixtensions to this inlude methods for the root to selfEselet lndmrks nd inorporte them into its mp QIF yther methods suh s prtile lters ITD SR llow the trking of nonEqussin distriutionsF sing tools suh s ulmn lter for loliztion hs eome stndrd method in moile rootisF eent works hve strted tking the root9s loliztion pility nd lndmrks into ount s prt of the pth plnning proessF vorussiD wrigoD nd fihi QS provided n elegnt nd detiled solution to the prolem of hoosing the pth of n exploring point root so s to optimize its ility to lolize in the presene of two preisely known lndE

R mrksF his method ssumes tht the lndmrks re perfetly knownD nd does not onsider unertin root motionF st lso does not inorporte movement towrds spei golF friggs et lF VD W use lndmrks in the presene of signint sensor unertintyF hey formulte the expeted shortest pth prolem s wrkov deision proessF heir method requires tht the visiility grph of the lndmrks e onstruted eforehndF ih edge of the visiility grph is ugmented with informtion out the proility tht the edge is pssle or notD iFeFD the trget lndmrk n e deteted from the urrent lndmrkF flei nd uelling U tke similr pprohD ut they ssume tht the stte of n edge is only known with ertin proilityF xeither pproh onsiders loliztion pility long the pthF vzns nd vtome PWD QH hve lso developed lgorithms for motion plnning with lndmrksF hey ssume tht lndmrks re res of perfet sensing nd perfet motion exeutionF hen the root is not within rnge of lndmrkD it is ssumed to hve ounded motion unertintyF hey use the ide of hining together sequenes of lndmrks tht the root n suessfully rehF hile their method llows unertinty in the root9s motionD their solution ignores ny issues of loliztion or sensing unertinty eetsF yther works hve proposed using some funtion of ovrine mtrix s ost funtionF rwny nd frfoot ST onsidered the est formtions for tem of roots to mintin loliztion ility when interEroot ommunition is llowedF hey use ost funtion equl to the determinnt of the ovrine mtrixF heir method does not inorporte externl lndmrksD ut the roots use eh other to perform loliztionF vogothetis et lF QQD QR propose using the tre of the trget stte error ovrine s the ost funtion in trget trking prolemF he im of their pproh is to nd the est pths for oservers tht n mesure only the ering to the trgetF hey solve the optimiztion prolem using oth dynmi progrmming nd rute fore enumertion over ll possile oserver pthsF feuse it is trget trking prolemD there is no notion of nl gol lotionF ezei et lF RQ introdued grphEsed lgorithm to pln the motion of vehile so s to inrese the overll informtion ontent in disrete loliztion mpF sn prtiulrD they use ost funtion tht is equl to the tre of the ovrine mtrixF ydometry nd inorportion of multiple sensing modlities ws not inluded in their workF vvlle et lF PUD PV hve developed some generl methods for motion plnning with

S unertinty|though their methods do not speilly tke loliztion unertinty into ountF hey speilly ddress the prolem of hnging environmentD suh s doors tht open nd lose ording to some sttistil proessD ut do not ddress soures of error suh s sensing nd odometry PVF foth of these methods use dynmi progrmming lgorithm to ompute root plnsF vmert nd prihrd PQ ddress the prolem of nvigting rElike root with motion nd sensing unertintyF heir ost funtion is omintion of pth length nd mesure of loliztion pility long the pthF heir pproh requires prior mp of the environmentF hey nd the fetures tht re est for loliztion in dierent regions of the mpF he regions of the mp tht shre the sme fetures re onneted using rodmpF he shortest pth is omputed using the hijkstr lgorithmIQF yne of the most losely relted prior works is the ostl nvigtion4 lgorithm of hrun nd oworkers RTD RUF his work similrly formulted ost funtion{sed lE gorithm to nd pths tht llow root to tke dvntge of previously onstruted gridEsed mp of n environmentF heir formultion resulted in ostly dynmi proE grmming @or poliy itertionA solutionF et rst glneD the prolem presented in this thesis ould e solved using dynmi progrmmingF roweverD the struture my formultion permits simplition of the solution to more eient optiml ontrol solutionF sn turnD it n e prtilly solved using simple ollotion nd grdient desent methodF
1.3 Contributions of this Thesis

his thesis introdues new lgorithm to estimte root9s plnr displement y weighted mthing of dense twoEdimensionl rnge snsF fsed on models of expeted sensor unE ertintyD the lgorithm weights the ontriution of eh sn point to the overll mthing error ording to its unertintyF e mximum likelihood formultion is used to estimte the optiml displement etween two onseutive posesF nertinty models tht ount for eets suh s mesurement noiseD sensor inidene ngleD nd orrespondene error re developedF his lso gives more relisti ovrine of the displement estimte thn is found in prior workF his work ws done jointly with muel ster nd tergios oumeliotisF ith some moditionsD the ngent fug lgorithm n e rest within n optimizE

T tion frmeworkF he originl ngent fug does not lwys follow the shortest pth to the golF e new lgorithm tht lwys follows the shortest pth to the golD given limited inE formtion out the worldD is developedF his new lgorithm is lled Optim-BugF s prove tht this new lgorithm is omplete nd orretF esults nd generl ides from yptimEfug re used to motivte disussion nd development of n lgorithm to hndle the se of noisy odometry nd imperfet sensingF en oEline optimiztion method ssuming omplete knowledge of the environment tht omputes the est pth to minimize root pose unertinty t the gol is presentedF fy minimizing the root9s expeted position error ovrine t the golD s m mximizing the possiility tht the root will e le to reognize the gol when its position estimte sys tht it is t the golF his optimiztion method is rst step towrds ridging the gp etween sensorEsed plnning lgorithms nd loliztion nd mpping tehniquesF e min ontriution of this work is sensorEsed plnning lgorithm lled Uncertain BugD in whih the root is not ssumed to hve perfet dedErekoning nd prior knowlE edge of the environment9s geometryF nertin fug lso tkes into ount noisy sensor mesurements nd unertin lndmrk lotionsF sssues suh s the hoie of lndmrks for nvigtionD or lgorithms to lern importnt lndmrks SQD re not ddressed in this thesisF he nertin fug lgorithm nds the pth to the gol tht minimizes the expeted root position unertinty t the golF st is ssumed tht if the root n get lose enough4 to the golD it will e le to reognize it nd delre suessF yn the other hndD if no pths exist tht reh the gol within this thresholdD the lgorithm delres filureF en interesting onsequene of this threshold is tht the root my fil to reh the gol even when n eptle pth existed from the strtF sn other wordsD the ft tht the lgorithm hose one eptle pth over nother mens tht it n no longer gurntee tht it will get lose enough to the gol to reognize itF
1.4 Thesis Outline

ghpter P provides kground mteril on ulmn lters nd the ngent fug lgorithmF st is ssumed tht the reder hs some knowledge of ulmn lteringD with min results presented for the se of moile rootF ghpter Q presents theory nd results from rnge

U sn mthing lgorithmF he min improvement over previous tehniques is tht the new lgorithm tkes into ount the unertinty of sensor mesurementsF ghpter R introdues yptimEfugD n lgorithm tht lwys follows the shortest pth to the gol in n unknown environmentF st is shown tht this new lgorithm is omplete nd orretF ghpter S introdues the detils of n optimiztion method used to nd the pth with the smllest root pose estimte ovrine t the golF foth results nd detils of the optimiztion pproh re presentedF pinlly ghpter T presents the nertin fug lgorithmF nertin fug plns the pth with the lest root pose estimte unertinty t the golF

Chapter 2
Background
2.1 Tangent Bug

his setion provides short desription of the ngent fug lgorithm developed y sshy umonD ilon imonD nd ihud ivlin in IWWS PHF wuh of the inspirtion for yptimEfug nd nertin fug omes from the ngent fug lgorithmF he im of ngent fug is to nd pth through previously unknown terrin from the root9s urrent lotion to some given gol lotionF he pth must not interset ny ostles @orretnessAF woreoverD the lgorithm must either reh the gol or determine tht the gol is unrehle in nite mount of time @ompletenessAF he root strts t lotion xR D nd is ommnded to move to position of xg F he root is ssumed to e equipped with n omnidiretionl sensor with mximum sensing rnge of RF ngent fug mkes use of the local tangent graphD or vqF he vq onsists of nodes t the root9s lotion nd the endpoints of sensed ostlesD nd edges etween the root nd the sensed ostle endpointsF en optionl nodeD Tg D is dded t the intersetion etween the line segment xR xg nd the irle t xR with rdius of RF Tg is dded if nd only if the line segment does not interset ny ostles nd d@xR ; xg A > RF Tg is the projetion of the gol onto the visile setD nd is dded only if the strightEline pth to the gol is freeF pigure PFI shows n exmple of wht the vq my look likeF sn this exmpleD the pth to the gol within the root9s visile region is unostrutedD so Tg is ddedF ngent fug opertes in two modesX motion to goal @wtqA mode nd boundary following @fpA modeF huring the motion to gol modeD the root monotonilly dereses its distne to the golF foundry following is used to espe lol minim in the distne to the gol funtionF he root is onstntly sensing the environment nd omputing the vqF he

W
LTG Node LTG Edge

xg
Tg

xR

pigure PFIX he lol tngent grphD with the optionl gol nodeD Tg D ddedF vq is then used to determine the next motionF ogether with the swithing onditionsD ngent fug gurntees tht the root will reh the golD if it is rehleF sn wtq modeD the root lwys moves loser to the golF et eh stepD the root onstruts the vqF xext the vq is serhed to nd the locally optimal diretion long whih the root should moveF st is lolly optiml in tht it is the shortest pth to the golD tking into ount only the ostle informtion tht is ville in the nite sensing rngeF wtq mode ontinues until the root rehes the gol or the root detets tht it is trpped within lol minimum of the distne to gol funtionF his sitution is used y n ostle loking the root9s pthF hen this hppensD the lgorithm swithes to oundry followingF en lterntive interprettion of the wtq mode is tht of n optiml ontrol prolemF gonsider n optiml ontrol prolem where the system stte @rootA strts t lotion xR D with the gol of moving to stte xg F he prolem is to nd the lowest ost pth tht moves from xR to xg D where the ost of pth is dened s the totl length of the pthF wotion to gol mode is equivlent to this optiml ontrol prolem when only the ostle informtion @onstrintsA in the urrent visiility set @denoted v@qAA is tken into ountD nd the root only exeutes the portion of the pth inside v@qAF he reliztion tht ngent fug n e

IH
xg

xR

pigure PFPX e sitution where the root would swith into oundry following modeF formulted s n optiml ontrol prolem motivtes the lgorithms in ghpters R nd TF foundry following mode is used to nvigte round the loking ostle tht prevents the root from mking further progress towrds the golF he root swithes to fp mode when there re no nodes of the vq tht re loser to the gol thn its urrent position|it is trpped in lol minim of the distne to gol funtionF en exmple of when fp mode must e used is presented in pigure PFP hen it swithes to fp modeD the root rememers dminD the smllest distne to xg from ny point in the urrently visile freespe t the strt of the urrent oundry following sequeneF st lso hooses diretion to follow the ostle oundryF hile following the ostle oundryD the vq is ontinully updted nd heked for the leaving conditionD node tht will move the root loser to the gol thn dmin F hen suh node is foundD the root moves to tht node nd swithes k to motion to gol modeF yf ourseD if the root hppens upon the gol while irumnvigting the ostleD the lgorithm termintesF yn the other hndD if the root ompletes loop round the ostle without stisfying the leving onditionD the gol is deemed unrehle nd the lgorithm termintesF ngent fug in summryX IF wotion to golX ghoose the lolly optiml pth towrds the golD until

@A he gol is rehedF topF @A e lol minimum of the distne to gol funtion d@; xg A is detetedF qo to oundry following modeF PF foundry followingX ik diretion to move round the ostleF wove round the ostle oundry while updting dminD the losest enountered distne to the gol so frD until @A the gol is rehedF topY @A the leving ondition is metF qo to motion to gol modeY @A the root detets tht the gol is unrehleF topF imilr to nerly ll other omplete nd orret plnnersD ngent fug ssumes tht the root hs perfet ded rekoningF ht isD the root knows extly where it is in the glol referene frme t ll times with no errorF eny relisti system is ound to hve noisy odometry from lowEqulity hrdwreD noisy sensorsD wheel slippgeD nd other ftorsF xo mtter how smll these errors reD they will grow without ound @s demonstrted in pigure PFSAF pigure PFQ depits smple run of the ngent fug lgorithmF sn this exmpleD the root9s odometry estimte is orrupted with qussin noise t eh stepF elthough the gol is reltively nerD the nl error is pproximtely IS7 of the totl distne trveledF
2.2 Kalman Filtering

II

his setion disusses the ulmn lter nd its use in rootisF e ulmn lter is n estimtor for liner system tht is orrupted y white qussin noiseF st is ssumed tht the reder hs some fmilirity with ulmn ltersD nd only short introdution to ulmn ltering is given in order to estlish nottion nd key formulsF efter the rief introdutionD results for the se of moile root tking mesurements of lndmrks re derivedF hese results re used extensively in lter setionsF

IP
Goal True Path Estimated Path Obstacle

10

10 0 5 10 X 15 20 25

pigure PFQX en exmple ngent fug sequene where the root thinks it is t the golD ut the error is lrgeF
2.2.1 The Basics

e disreteEtime ulmn lter estimtes the stte x P Rn of liner system whose evolution is desried y the disreteEtime liner eqution
x@k C IA a Fk x@kA C Bk u@kA C Gk wk

@PFIA

from mesurements z P Rm tht re ssumed to e relted to the stte y the eqution

z @k C IA a Hk+1 x@k C IA C vk+1 :
+1

@PFPA

he rndom vriles wk P Rp nd vk P Rm re the proess noise nd the mesureE ment noiseD respetivelyF hey re ssumed to e independentD whiteD nd qussin noise

proesses with the following propertiesX

IQ
E w k

a H; E wk wk a Qk ; E vk a H; T a R E vk vk k
T
+1 +1 +1

+1

@PFQA @PFRA @PFSA @PFTA

where E denotes the expettion opertionF xote tht the system nd mesurement mtriesD F P Rnn; B P Rnl ; G P RnpD nd H P Rnm need not e onstntF st is lso ssumed tht u@kA is known t eh time stepF he ulmn lter proeeds in two min stepsD the propagation step nd the update stepF he propgtion step uses the system dynmis to predit new stte estimte t the next time stepF ine the system is noisyD this predition will hve some error ssoited with itF he updte step uses the mesurements z to help orret the estimteF ine mesurements re lso orrupted y noiseD this orretion will not mke the estimte perfetF vet x@k=j A e the estimte of the stte t time kD using mesurements up to time j F he sme onvention will lso e used to denote the dependene of the stte ovrine on the time nd mesurement indiesF he stte x@k C I=kA n e onsidered to e the prior @efore the mesurement informtion is inorportedAD while x@k C I=k C IA is nlogous to the posterior @fter the mesurement informtion hs een usedAF he est estimte of the stte t the next time step ssumes tht wk tkes its zero menD
x@k C I=kA a Fk x@k=kA C Bk uk :

@PFUA

feuse the rel system is orrupted y the unknown noiseD the estimte will lwys hve some errorF vet the prior estimte error t time k C I e denoted y x@k C I=kAD ~
x@k C I=kA a x@k C I=kA x@k C I=kA: ~

@PFVA

he ovrine of the estimte error fter the propgtion stepD denoted P @k C I=kAD is

given y
P @k C I=kA

IR a a
E x@k C I=kA~T @k C I=kA ~ x

T Fk P @k C I=kAFk C Gk Qk Gk :

@PFWA @PFIHA

et every time step of system evolutionD the estimte nd the estimte error ovrine re propgtedF sf no mesurements re inorported to redue the estimte errorD the estimte error ovrine P @k CI=kA will grow without oundF he updte step of the ulmn lter redues the estimte error ovrineF sn deriving the updte equtionsD the gol is to nd n eqution tht gives posterior estimteD x@k CI=k CIAD s omintion of the priorD x@k CI=kAD nd weighted dierene etween true mesurementD z@k C IAD nd predited mesurementD Hk x@k C I=kAX
+1

x@k C I=k C IA a x@k C I=kA C Kk+1 @z @k C IA Hk+1 x@k C I=kAA :

+1

@PFIIA

he quntity @z@k C IA Hk x@k C I=kAA is termed the residualF he mtrix K is the Kalman gainD nd is found y minimizing the posterior estimte error ovrineF he resulting K n e written in mny formsF yne suh form is
1 T Kk+1 a P @k C I=kAHk+1 Sk+1 ;
+1 +1

@PFIPA

where the mtrix Sk P Rmm is the ovrine of the residul @z@k C IA Hk x@k C I=kAAX
Sk+1

a a

E @z @k C IA Hk+1 x@k C I=kAA @z @k C IA Hk+1 x@k C I=kAAT

T Hk+1 P @k C I=kAHk+1 C Rk+1 :

@PFIQA @PFIRA

he eqution desriing the posterior error ovrineD P @k C I=k C IAD n lso tke mny formsF yne form is given y
P @k C I=k C IA a P @k C I=kA Kk+1 Hk+1 P @k C I=kA:

@PFISA

here re mny possile pprohes to estimting the stte if the system dynmis re nonElinerF sn the most strightforwrd pprohD the nonEliner system is linerized out

IS the urrent men nd ovrineF he mtries from the lineriztion re then used s oveF his is referred to s n Extended Kalman lterD or iupF por thorough review of ulmn ltersD the reder is referred to QWF
2.2.2 Kalman Filters and Robotic Localization

he following setions review the use of the ulmn lter for moile root nvigtionF pigE ure PFR depits the si setupF he root is loted t position xR D nd it n mesure the rnge d ndGor the ering  to eh lndmrkD denoted xLiF he rnge nd erE ing mesurements llow the root to lulte the reltive position etween itself nd the lndmrkF
xL1 xL2

xL3

d xR 

xL4

pigure PFRX etup of ulmn lter for moile root loliztionF

2.2.2.1 Localization of a Point Robot Using Landmarks

sn the se of point root operting in grtesin workspeD the motion model ssumes the root hs omnidiretionl motion pilitiesF he root is given knowledge of the lotion of N lndmrksD s well s the ovrines of these lndmrk lotionsF st is ssumed tht the root n solve the data association prolem PPD iFeFD the root n distinguish whih lndmrk it is looking tF sf this is not the se nd the root nnot tell

IT lndmrk e from lndmrk fD then multiple hypothesis methods must e used TF he stte vetor x ontins the positions of oth the root nd ll of the lndmrksD iFeFD
x a xR xL1
h

xLN ;

iT

@PFITA

where xR is the grtesin position of the root nd xLi is the grtesin position of the ith lndmrkD P Q P Q xLi S xr S xR a R nd xLi a R : @PFIUA
yr yLi

he disrete time kinemti eqution for the root9s movement model is

xR @k C IA a xR @kA C V @kAt;

@PFIVA

where t is the time step etween disrete motionsD nd V @kA represents the root9s veloity t the kth time intervlX P Q vx @kAS V @k A a R ; @PFIWA vy @kA nd vx@kA nd vy @kA represent the trnsltionl veloities t time step k in the x nd y diretionsD respetivelyF his model ssumes tht veloity is onstnt in etween smplesF sing mesurements vi the root9s internl odometryD inertil nvigtion unitD qD snE mthing QUD RPD or other mens D one n propgte the estimte of the root9s stte with the following equtionX
1

xR @k C I=kA a xR @k=kA C Vm @kAt; where

vx @kAS Rwvx @kAS Vm @kA a V @kA C wV @kA a R C vy @kA wvy @k A
P Q P Q

@PFPHA @PFPIA

1 Some of the methods that provide additional input to the state estimation process may require that additional states be added to the system state dened in Equation (2.16).

IU re the mesurements of the root9s trnsltionl veloitiesF hese re orrupted y indeE pendent zeroEmen white qussin noise wV @kA with ovrine
P

T Q@kA a E wV @kAwV @kA

 2 @k A R vx

H S: vy @kA
2

@PFPPA

he N lndmrks re ssumed to e in xedD ut possily unertinD positionD iFeFD xLi @k C IA a xLi @kA;
i a I; :::; N;

@PFPQA

where xLi is the estimte of the ith lndmrk9s positionF fy inspetionD the F nd G mtries re
F G GR

a a

I(2N +2) ;
P R

H N P Q t H S : a R H t
2 2

GR

Q S;

@PFPRA

he zeros in the lower prt of the G mtrix reet the ssumption tht the lndmrks re xedF he estimte error ovrine mtrix will ssume lok strutureX
PRR PRL S P aR ; PLR PLL
P Q

@PFPSA

where PRR is the P P ovrine mtrix of the root9s position errorD PLL is the PN PN T mtrix of lndmrk position error ovrinesD nd PRL a PLR re the rossEoupling error ovrinesF st is not ssumed tht the lndmrks re known perfetlyD so PLL need not e ll zerosF uessive use of iqutions @PFPHA nd @PFIHA will propgte the estimte of the root nd lndmrk sttes forwrd in timeF he root n mesure the reltive position etween itself nd lndmrk i t ny

timeX
z @k C IA

IV a a
xLi @k C IA xR @k C IA C n@k C IA
P R Q P Q

xLi xr S @k C IA C RnxS @k C IA; yLi yr ny

@PFPTA @PFPUA

where n@k C IA is zeroEmen white qussin noise proess with ovrine

R@k C IA a E n@k C IAn@k C IAT :

@PFPVA

he H mtrix is then
H @k C IA a
h

I H
2

2 2(

i1)

I2

2 2(

N i)

@PFPWA

ith the H mtrixD the ulmn gin @iqution @PFIPAA n e omputedF he ulmn gin n then e used to updte the stte estimte nd the estimte error ovrineF pigures PFS nd PFT show representtive dt for the propgtion of ulmn lterF pigure PFS depits single movement sequene where the root tkes three stepsF sn this spei runD the root9s position estimte from the rst step is prtiulrly dF his exmple ws hosen to illustrte how lrge the errors n eF st lso shows how the estimte ovrine grows fter eh stepF pigure PFT presents n exmple where the root tkes single step SHH times from the sme strting positionF elthough the strting position nd the true nl positions re the sme for eh trilD the noise vlues tht orrupt the root9s veloity @iqution PFPIA re dierent for every runF husD the nl estimtes re ll slightly dierentF pigure PFT lso shows zoomedEin view of ll of the nl estimtes for the sme SHH trilsF sf the numer of trils were inresedD the nl estimte verge would move loser nd loser to the true nl positionF he WWFU 7 ovrine ellipse plotted in the gure is the men of the ovrines for ll trilsF es expetedD most of the nl estimtes lie within this ellipseF pigure PFU shows representtive results from the use of the updte step of ulmn lter to lolize the rootF sn this exmpleD the root tkes two stepsF he sequene inludes oth sensing noise nd lndmrk positionl unertintyF efter the rst stepD the lndmrk is not within the root9s sensing rngeF husD the updte step does not orret the root9s

IW
4.5 4 3.5 3 2.5 Y 2 1.5 1 0.5 0 1 0 1 2 3 4 5 True Position Position Estimate 99.7% Covariance Ellipse

pigure PFSX e threeEstep propgtion exmpleF his shows how the estimte error ontinues to growF
2.5 Start Position Final Estimates
2.4 True Final Position Final Estimates Final Estimate Average 99.7% Error Ellipse

2.3 2.2

1.5 Y

2.1 2 1.9 Y

0.5

1.8 1.7

0 0.5 0 0.5 1 1.5 2 2.5 3

1.6 1.4 1.6 1.8 2 X 2.2 2.4 2.6

pigure PFTX e singleEstep propgtion exmple showing multiple trilsF position estimteF yne n see tht oth the preEupdte nd postEupdte estimtes re the smeD nd the Q ovrine ellipses re lso the smeF efter the seond stepD howeverD the root is lose enough to the lndmrk to use it for loliztionF feuse of odometri errorsD the root9s preEupdte estimte error is well over HFS unitsF efter the updteD the solute error in the estimte is gretly reduedD s is the estimte error ovrineF he

PH updte step lso redued the error ovrine in the lndmrk position estimte @not shown in the gureAF hen using ulmn lter to lolize moile rootD there is nothing speil out the root reltive to the lndmrksF he root n e thought of s moving lndmrk for whih the dynmis re knownF
True Position 1 Preupdate Estimates Postupdate Estimates Landmark True Position 0 Landmark Estimate Sensor Range

1 Y 2 3 4 0 1 2 3 X 4 5 6

pigure PFUX ixmple dt illustrting using lndmrk for loliztionF

PI

Chapter 3
Weighted Scan Matching
3.1 Introduction

his hpter introdues weighted rnge sensor dt mthing lgorithm to estimte root9s displement etween the ongurtions where dense twoEdimensionl rnge sns re otinedF his novel lgorithm tkes into ount severl importnt physil phenomen tht et rnge sensing ury tht hve een negleted in prior workF he experiments in etion QFT show tht this lgorithm is not only eientD ut ppreily more urte thn nonEweighted mthing methodsD suh s tht of efF QUF woreoverD y omputing more relisti ovrine of the displement estimtesD the weighted mthing lgorithm provides etter sis for fusion of these estimtes with odometri ndGor inertil meE surements RSF he fused estimtes n susequently e used to support loliztion nd mpping tsksF his work ws performed jointly with muel ster nd tergios oumeE liotisF o understnd the ontent of this hpter nd its ontriutions estD the si proE lemD how the solution diers from previous onesD nd the generlity of this pproh re desriedF he fous is on moile roots operting in plnr environmentsF st is ssumed tht the root is equipped with dense plnr rnge sensor @eFgFD lser rnge snnerAF es disussed in etion QFPFRD onEord odometry is usefulD ut not essentilF he root strts t n initil ongurtionD g D nd moves through sequene of onE gurtionsD giD i a P; : : : ; mF rere gi P SE @PA denotes the root9s position nd orienttion reltive to xed referene frmeD g F st is ssumed tht t eh poseD the root mesures the rnge to the oundry of its nery environment long rys tht re seprted y
1 0

PP
+ j i uk+1 i uj uk+1 X j u k uik1uk1 k X X jk ik i

+
i

Wall

Range Measurements

Pose i

y i x i
i k

k yj x j

Pose j

pigure QFIX qeometry of the rnge sensing proessF he root quires dense rnge sns in poses i nd j F he irles represent root positionD while the xEy xes denote the root9s ody xed referene frmesF uniform ngleD  @see pigure QFIAF es desried elowD vrious unertinties in this rnge mesurement re ounted forF vet the set of grtesin oordintes of the ni sn points tken in the ith root pose e denoted y f~ ik gD k a I; : : : ; niF he sn point oordintes re desried in the root9s u ody xed referene frmeF ypillyD the grtesin oordinte of the sn point is derived from rnge dt ording to the expression
1

i i i a Rxk S a li Ros k S ; ~k u k i i yk sin k

@QFIA

i where lk is the mesured distne to the environment9s oundry long the kth mesuring i i ryF he mesuring ry is oriented in the diretion denoted y k D where k is the ngle mde y the kth mesuring ry with respet to the xExis of the ody xed referene frme @see pigure QFIAF he min gol is to urtely estimte the root9s displement etween poses y mthing rnge dt otined in sequentil posesF his displement estimte n e used s the sis for form of odometryD or fused with onventionl odometry ndGor inertil mesurements to otin etter reltive root pose estimtesF sn turnD these estimtes n

1 The extension to non-uniform angle  is straightforward.

PQ support loliztion nd mpping proeduresF pirstD ssume tht the rnge sns t poses i nd j hve suient numer of orresponding points to e suessfully mthed @see etion QFRAF vet f~ ik ; ~ jk g for k a I; : : : ; nij e the set of orresponding mthed sn u u point pirsD where nij is the numer of orresponding pirsF prom these pirsD the reltive displement etween poses i nd j X gij a gi gj a @Rij ; pij A; will e estimted where
1

Rij a R

os ij sin ij S ; sin ij os ij

xij pij a R S ; ~ yij

@QFPA

iFeFD the displement etween poses i nd j is desried y trnsltionD @xij ; yij AD nd rottionD ij F xextD the ovrineD P ij D of the displement estimte is lultedF his ovrine hs two min usesF pirstD it reets the qulity of the displement estimtesF vrge digonl elements of the ovrine mtrix indite inresed unertintyF eny loliztion proess should e wre of the level of ondene in its omputed pose estimtesF eondD the ovrine is needed when omining displement estimtes with mesurements provided y other sensorsF wore urte nd relisti estimtes of the ontriuting ovrines led to more urte overll estimtes in sensor fusion lgorithmD suh s ulmn lterF his pproh diers from prior work in tht the ontriution of eh sn point to the nl displement estimte is individully weighted ording to tht point9s spei unertintyF he sn point unertinties re estimted using sensor mesurement noise modelsD s well s models of spei geometri issues within the mthing proess itselfF pigures QFI nd QFP illustrte these issuesF pigure QFI depits sitution where rnge sensor @eFgFD lser rnge nderA smples points on nery wllF he oundry points smpled in pose i re indited y irlesD nd leled y ~ ik D ~ ik D nd ~ ik F he nery oundry u u u points smpled in pose j re indited y X 9s nd re leled y ~ jk D ~ jk D nd ~ jk F rior u u u rnge mthing methods @eFgFD IRD IUD SUA hve mde the simplifying ssumption tht the rnge sns of dierent poses smple the environment9s oundry t exactly the sme points|iFeFD point ~ ik is ssumed to e extly the sme point s ~ jk D etF This assumption u u is generally not trueF rereD this correspondence error is modelled nd its eet inorported into the mthing lgorithmF es desried in etions QFQFI nd QFQFQD the rnge mesurements re orrupted y
1 +1 1 +1

PR

Scan Points Selected Scan Points 100 x Point Covariance (3) 6000 4000 2000
(mm) Robot Pose

2000

4000

pigure QFPX epresenttion of the unertinty of seleted rnge sn pointsF

i noise nd possily is term tht is funtion of the rnge sensing diretionD k D nd i the sensor em9s inidene ngleD k @pigure QFIAF pigure QFP shows the WS7 ondene level ellipses ssoited with the ovrine estimtes @lulted using the methods tht re introdued lterA of seleted dt points from n tul lser rnge snF he wide vrition in unertinties seen in pigure QFP strongly suggests tht not ll rnge dt points re of equl preisionF reneD the potentilly lrge vriility should e tken into ount in the estimtion proessF hile the existene of these unertinty soures hs previously een suggested ID PD QD SD IRD this lgorithm is the rst to expliitly model nd ount for their eets within the estimtion proessF ome prior works hve no expliit noise modeling @eFgFD IUAD or pply uniform unertinty to ll ontriuting pointsF he most omplete existing methodsD S nd QTD employ sttistil methods to lulte displement estimte unertintyF hese methods do not tke sensor unertinty models into ount in the displement estimtion proessD nd use n unweighted ssumption for the ontriuting pointsF elsoD S nd QT do not use ny spei sensor noise hrteristis s sis for lulting unertintyF snstedD they use numeril smple of perturtions to extrt n estimte of ovrineF ignint improvements over previous unweighted methods re demonstrted y developing physilly sed unertinty models for eh individul point nd inorporting these models in oth the displement estimtion proess nd the ovrine lultionF

PS he si priniple ehind this new pproh generlly pplies to ny se of dense rnge dtD suh s sonrsD infrredsD mersD rdrsD etF he si weighted mthing formultion nd its solution given in etion QFP re independent of ny sensor speisF o use the generl resultsD spei models of sensor unertinty re neededF hese detiled sensor models re developed in etion QFQF ine some of the ssumptions underlying these sensor models re est suited to lser rnge snnersD the pplition of the detiled sensor model formuls is est suited to the use of lser snners in indoor environments @though they n e extended to strutured outdoor environmentsAF roweverD the generl pproh of etion QFP should work for other rnge sensors nd other operting environments with resonle moditions to the sensor modelsF his hpter is strutured s followsX etion QFP desries generl weighted point feture mthing prolem nd its solutionF etion QFQ develops orrespondene nd rnge mesurement error modelsF etions QFR nd QFS summrize the point piring seletion nd sensor inidene ngle estimtion proeduresF ixperiments in etion QFT demonstrte the lgorithm9s uryD roustnessD nd onvergene rngeF hiret omprisons with previous methods @eFgFD QTD QUA vlidte the eetiveness of this pprohF
3.2 The Weighted Range Sensor Matching Problem

his setion desries generl point feture mthing prolem nd its si solutionF
3.2.1 The Measurement Model

vet the sets of grtesin rnge sn dt points quired in poses i nd j e denoted y f~ ik g nd f~ j gD respetivelyF hese mesurements will e imperfetF vet f~k g nd f~k g e u uk ri rj the true4 grtesin sn point lotionsF he mesurements n generlly e deomposed into the following termsX
~i uk ~j uk

a a

~k C ~ i C ~ i ri uk bk ~k C ~ j C ~ j ; rj uk bk

@QFQA

PT where ~ ik nd ~ jk represent noise or unertinty in the rnge mesurement proessD nd u u j denote the possile rnge mesurement isF4 hese noise nd is terms re ~ i nd ~ k bk b disussed in more detil in etions QFQFI nd QFQFQF he term ~ k is typilly well modelled u y zeroEmen qussin noise proessF he is ~k is n unknown oset tht n e b pproximted y termD ~k orrupted y zeroEmen dditive qussin noiseD ~k PF o b he ovrine of this noise omponent reets the level of ondene in the vlue ~k F o gontingent on this pproximtionD ~ik nd ~jk tke the form b b
2

~ i a ~i C ~ i Y bk ok bk

~ j a ~j C ~ j : bk ok bk

@QFRA

vet @~ ik ; ~ jk A e points tht orrespond in the rnge sns t poses i nd j F es shown u u in pigure QFID these points re not neessrily the sme physil pointD ut the losest orresponding pointsF eounting for the ft tht sn dt is mesured in rootExed frmeD the error etween the two orresponding points is
uk "ij a ~ i Rij ~ j pij uk k

@QFSA

for given displement @Rij ; pij A etween posesF ustituting iqution QFQ into iqution QFS results in
uk bk uk bk "ij a @~k Rij ~k pij A C @~ i Rij ~ j A C @~ i Rij~ j A : ri rj k
| {z
( )

{z

( )

ii

{z

iii)

@QFTA

e reltive pose estimtion lgorithm ims to estimte the displement gij a @Rij ; pij A tht suitly minimizes iqution QFT over the set of ll orrespondenesF sf the dense rnge sns do smple the ext sme oundry pointsD then ~k Rij ~k pij a H when Rij nd ri rj pij ssume their proper vluesF roweverD ~k nd ~k generlly do not orrespond to the sme ri rj oundry pointF hereforeD term @i A in iqution QFT is the correspondence errorD denoted y cij X k cij a ~k Rij ~k pij : ri rj @QFUA k he mthing error "ij for the kth orresponding point is lso funtion ofX @ii A the error k due to the mesurement proess noiseD nd @iii A the mesurement is errorF
2 The value of ~k can be determined by statistical analysis of measurement data. o

PU por the ske of simpliityD the is osets re ignored for now @iFeFD ssume tht ~ik a b j aHAD ut their eet will e onsidered gin in etion QFQFQF ~k b
3.2.2 A General Covariance Model

por susequent developmentsD generlized expression for the ovrine of the mesureE ment errors is neededX
ij Pk

R a a

E "ij @"ij AT k k
h

i i

E @cij C ~ i Rij ~ j A@cij C ~ i Rij ~ j AT ; uk uk k uk uk k

@QFVA

where E is the expettion opertorF ell tht is eets re ignored for nowF Pkij ptures the unertinty in the error etween orresponding rnge point pirsF feuse the rnge mesurement noise is ssumed to e zero menD qussinD nd independent ross mesurementsD E ~ ik @~ jk AT a E ~ jk @~ ik AT a HF rtilly spekingD one would expet u u u u tht the rnge mesurement noise of the kth sn point in pose i to e unorrelted with the mesurement noise of the kth orresponding rnge point in pose j F hereforeD this is ne ssumption in prtieF he orrespondene errorD cij D is deterministi vrile tht is funtion of the geE k ometry of the root9s surroundingsF roweverD sine the geometry of the environment is not ssumed to e known hed of timeD resonle probabilistic approximation is mde to this termD whih ounts for the ft tht the geometry of the surroundings is unE known prioriF sn this proilisti pproximting modelD the orrespondene error nd sensor mesurement error terms re independentF hereforeD E cij @~ ik AT a E cij @~ jk AT a k u k u E ~ i @cij AT a E ~ j @cij AT a HF ee etion QFQFP for more detiled disussionF uk k uk k nder these ssumptionsD the ovrine of the mthing error t the kth point orreE spondene of poses i nd j eomesX
ij Pk

R a C a a

E "ij @"ij AT k k

a E cij @cij AT C E ~ ik @~ ik AT u u k k h i T Rij E ~ j @~ j AT Rij uk uk C P ij C N P i C R N P j RT ij k ij k k ij T Qij C Rij Sk Rij k

@QFWA @QFIHA

where
C P ij k NPi k NPj k Qij k ij Sk

PV a ovrine ssoited with the pproximting orrespondene error model; a mesurement noise ovrine of the kth sn point in the ith pose; a mesurement noise ovrine of the kth sn point in the j th pose; R a C Pkij C N Pki ; R a N Pkj :

ij he mtries Qij nd Sk represent the ongurtionEindependent nd ongurtionEdependent k terms of Pkij F es shown elowD the orrespondene errors depend on the sensor em9s inE i j i idene ngleF he noise ovrines will lso e funtion of the vriles k D k D lk D nd j ij lk F husD the ovrine mtrix Pk is expeted to vry for eh sn point pir @see pigure QFP for n illustrtionAF st is not suitle to ssume tht Pkij is onstnt mtrix for ll sn point pirsD s hs een done in prior work @eFgFD QTD QUAF

3.2.3 Displacement Estimation via Maximum Likelihood

e mximum likelihood @wvA frmework is used to formulte generl strtegy for estimtE ing the root9s displement from set of nonEuniformly weighted point orrespondenesF vet v@f"ij gjgij A denote the likelihood function tht ptures the likelihood of otining the k set of mthing errors f"ij g given displementD gij F nder the ssumptions oveD the k k a I; : : : ; nij rnge pir mesurements re independent nd the likelihood n e written s produtX v@f"ij gjgij A a v@"ij jgij Av@"ij jgij A v@"ijij jgij A: @QFIIA n k
3 1 2

ell tht the mesurement noise is onsidered to e zeroEmen qussin proessF piE nllyD s shown in etion QFQFPD the orrespondene noise n e pproximted y zeroE men qussin proessF xegleting the is oset for the moment @see etion QFQFQAD the
3 Possible dependencies of these measurements will be brie y considered in Section 3.3.2. Generally, the only eect that will lead to dependence is possible couplings in the correspondence error that arise if the geometry of the environment is a priori known.

PW ove ssumptions imply tht v@f"ij gjgij A tkes the form k

v@ f"ij gjgij k

A a

e 2 ("k ) (Pk ) "k q ij k=1 P det Pk

ij T ij

nij

ij

eM ij ; Dij

@QFIPA

where
M ij Dij
nij I @"ij AT @P ij A "ij ; a P k k k k
1

@QFIQA @QFIRA

nij

=1

k=1

P det Pkij :

he optiml displement estimte is the one tht mximizes the vlue of v@f"ij gjgij A k with respet to displementF yne n use ny numeril optimiztion sheme to otin this displement estimteF xoteD howeverD tht mximizing iqution QFIP is equivlent to mximizing the logElikelihood funtionX
lnv@f"ij gjgij A a M ij ln@Dij A: k

@QFISA

prom numeril point of viewD it is often preferle to work with the logElikelihood funE tionF fefore disussing the solution to this estimtion prolemD this formultion is ompred with prior workF wost prior lgorithms tht tke n unweighted4 pproh to the displeE ment estimtion prolem ssume tht ll of the ovrine mtries Pkij re uniformly the P P identity mtrixF gonsequentlyD the mximiztion of the logElikelihood funtion redues to stndrd lestEsqures prolemF roweverD s pigure QFP nd experiments in etion QFT showD suh simplisti ovrine pproximtion for ll dt points is typilly not theoretilly sound oneF elthough SU llowed for slr weighting termD no guidne ws provided on how to selet the vlue of the slrF he weighted estimtion prolem hs some inherent struture tht leds to eieny in the mximiztion proedureF eppendix eFI proves tht the optiml estimte of the root9s trnsltion n e omputed using the following losed form expressionX

Proposition 3.1. The weighted scan match translational displacement estimate, pij , is

QH

pij a Ppp

nij  k=1

@Pkij A @~ ik Rij ~ jk A u u
1

@QFITA

where Rij

ij a Rij @ A is the estimated rotational matrix calculated with the current estimate of the orientation displacement ij , and Ppp is given by the formula
Ppp a
2n ij

k=1

@ A
ij Pk

31
1

@QFIUA

en ext losed form expression for estimting the rottionl displement ij does not existF xonethelessD there re two eient pprohes to omputing this estimteF sn the rst pprohD the trnsltionl estimte of iqution QFIT is sustituted into iqution QFIP @or equivlentlyD into iqution QFISAF ine the resulting expression is funtion of the single vrileD ij D the estimtion proedure redues to numeril mximiztion over single slr vrileD ij D for whih there re mny eient lgorithmsF elterntivelyD one n develop the following seond order itertive solution to the nonE liner estimtion prolem @eppendix eFPAX
Proposition 3.2. The weighted scan match rotational displacement estimate is updated as ij ij + a  C ij , where nij ij =1 k ij 9 niij pTT @Pk ijA1Jqk ; @QFIVA  1 k=1 qk J @Pk A Jqk
where

u H I S ; qk a Rij ~ jk Ja @QFIWA uk : I H pk a ~ i pij Rij ~ j uk sing vrious experimentl dtD this pproximtion hs een found to gree with the ext numeril solution up to ve signint digitsF purthermoreD it is omputtionlly more eient to implementF
R

3.2.4 The Algorithm and Its Initial Conditions

ropositions QFI nd QFP suggest n itertive lgorithm for estimting displementF en ij initil guess  for ij is hosenF e trnsltion estimte pij is omputed using roposition QFIF his estimte n e used with n ext numeril optimiztion proedure or with

QI ij roposition QFP to updte the urrent rottionl estimte  F he improved ij is the sis for the next itertionF he itertions stop when onvergene riterion is rehedF ij he initil guessD  D will usully e derived from n odometry estimteF roweverD odometry is not neessry for the method to workF en open loop estimte of the root9s displement sed on the known ontrol inputs tht generte the displement will often provide suient ury for n initil guessF st is shown in etion QFTFI tht the lgoE rithm9s performne is not hmpered y lrge errors in the initil vlue of the displement used s seed for the lgorithmF xote tht if odometry does provide the initil guessD there will e no orreltion etween the estimte rising from the sn mthing lgorithm nd the odometry estimte sine the ury of the ltter is not onsidered in the estimtion proessF his simplies susequent fusion of these estimtesD whih my e desired for some pplitionsF en itertive lgorithm is preferred for two resonsF pirstD nonEliner wv prolems re suited to itertive omputtionF eondD the orret orrespondene etween point pirs nnot e gurnteed in the point orrespondene prolem @see etion QFRAF his is espeilly true in the rst few lgorithm itertionsD where some inurte initil pirings re unvoidleF he itertive pproh llows for ontinul redjustment of the point orrespondenes s the itertions proeedF
+

3.2.5 Covariance of the Displacement Estimation Error

vetting pij a pij pij ; ij a ij ij @iFeFD pij ; ij re the trnsltionl nd the rottionl ~ ~ ~ ~ displement error estimtesAD diret lultion yields the followingX
Proposition 3.3. The covariance of the displacement estimate is
P

P ij a R

Ppp Pp Pp P

Q S

E fpij pT g ij R E fij pT g ij

~ ~ ~ ~

E fpij T g S ~ ~ij ~ ~ij E fij T g

QP
with

Pp Pp P rT

a rI

2n ij

a a rI ;
T

T k=1 T Pp ; nij k=1

ij Pk

A1

31 n ij 

k=1

@Pkij A Jqk
1

@QFPHA @QFPIA @QFPPA @QFPQA

ij T qk J @Pk A1 Jqk ;

and Ppp is given by Equation 3.17.

he proofs for roposition QFQ re given in eppendix eFQF por given sensorD one must derive pproprite unertinty modelsD whih re then sustituted into the ove proedureF he mtrix J @Pkij A J a Pkij Pkij in iqution QFPQ is positive denite mtrixF herefore P is positive numerF ij Note 2: prom iqutions QFPP nd @QFPQAD for ounded ovrine @k@Pk A k < K ; H < K < IAX
1 1 det( ) 1

Note 1X

k~ j k3I uk

lim

P a

lim P a H: kqk k3I 

his result leds to the following orollryX

Corollary 3.4. Matching of distant features (in the limit features at innite distance from
the current location) minimizes the expected error in the orientation displacement estimate. In the limit, the relative orientation error is zero.

ine ll mtries Pkij ; k a I; : : : ; nij D in iqution QFIU re positive deniteD the ovrine of the trnsltionl estimteD PppD n e written s @PppA1 a
nij k=1 ij Pk

Note 3X

@Pkij A

ij > @Pk A1 D

Ppp <

; k a I; : : : ; nij :

@QFPRA

rere the nottion X > Y indites tht the dierene X Y is positive denite mtrixF iqution QFPR leds to the following orollryX

ij Corollary 3.5. Let U ij a mink=1;:::;nij Pk denote the minimum covariance over all corresponding point pairs. The translational covariance estimate, Ppp , given by Equation 3.17 is bounded above by U ij : Ppp < U ij .

QQ

his orollry sttes tht the ovrine of the trnsltionl estimte will lwys e less thn the est single ovrine ssoited with ny orresponding point pirF
3.3 Scan Matching Error/Noise Models

sn order to derive expliit expressions for the ovrines of iqution QFIHD this setion develops models for the errors inherent in the rnge sn mthing proessF wost of the models re quite generlD though few ssumptions re mde t some points tht re most pproprite for lser rnge snnersF
3.3.1 Measurement Process Noise

wny rnge sensing methods re sed on the time of ight @eFgFD ultrsound nd some lser snnersA or modultion of emitted rdition PD QF he iruits governing these mesurement methods re sujet to noiseF hese eets n often e well modelled in simple wyD enling the omputtion of the ovrine ontriutionsD N Pki D nd N Pkj F he omputtion of N Pki is foused onD s the one for N Pkj is ompletely nlogousF ell the polr representtion of sn dtD iqution QFIF vet the rnge mesurementD i i lk D e omprised of the true4 rngeD Li D nd n dditive noise termD "l X lk a Li C "l F he k k noiseD "l D is ssumed to e zeroEmen qussin rndom vrile with vrine l @see eFgFD efF P for justition of this ssumptionAF elso ssume tht error or unertinty exists i in the mesurement k F ht isD the tul sn ngle diers from the reported or ssumed i ngle of the sn snpshotF husD k a ik C " D where ik is the true4 ngle of the kth sn diretionD nd " is gin zeroEmen qussin rndom vrile with vrine  F reneX
2 2

i i i a Li R os k S a @li " A R os@k " A S : ~k r l k k i sin ik sin@k " A

@QFPSA

por smll " D "l @whih is good pproximtion for most lser snnersAD expnding iqution

QR QFPS nd using the reltionship ~ ik a ~ ik ~k yields u u ri

P

i ~ i a @lk A" uk

i sin k S R i os k

i C "l Ros ik S : sin k

@QFPTA

essuming tht " nd "l re independentD then

NPi k

E ~ i @~ i AT a uk uk
P
2 2

@ A  R P sin P sin P
i lk
2 2 2

i i C Pl RP os i k sin Pki S : sin Pk P sin k

2

i k i k

sin P

P os

i k S 2 i k

@QFPUA

i i he quntities k nd lk re the ones mesured y the lser snnerF

3.3.2 Correspondence Error

rereD the orrespondene error desried in etion QFPFI is nlyzedD nd proilisti pproximtion to this error is derivedF he derivtion ssumes tht the sensor em strikes n environmentl oundry tht is lolly strightE line segment @pigure QFIAF roweverD this derivtion n e extended to other oundry geometriesD or it n serve s n exellent tngent pproximtion for modertely urved oundriesF e rst develop formul for the mximum possile orrespondene error tht n our due to the ft tht the ext sme oundry points re not smpled in two suessive rnge snsF gonsider how nery sn points will e mthed in the viinity of points ~ ik nd ~ jk u u in pigure QFIF vet i i a jj~ i ~ i jj;  a jj~ i ~ i jj uk uk uk uk @QFPVA
+ +1 1

denote the distne to the djent sn points @from pose i9s snA ner the ndidte j mthing point ~ ik @see pigure QFIAF imilrlyD let j a jj~ jk ~ jk jj nd  a jj~ jk ~ jk jj u u u u u denote the distnes to the djent sn points @from pose j 9s snA ner the ndidte mthing point ~ jk F he mximum distne @or errorA etween ny pir of points tht re u hosen to e in orrespondene will e hlf of the minimum distne etween djent sn pointsF sf the error is greter thn this vlueD the point will e mthed to nother pointD i or it will not e mthed t llF yn vergeD this error will e the minimum of @i C A=R
+ +1 1 +

QS j or @j C A=RF imple geometri nlysis of pigure QFI shows tht

+

i i + C 

i i R sin@k C  A C sin@k  A ! i os i a lk sin  sinsink sin  : i P k

2 2 +

i lk sin 

@QFPWA

j ustituting j for i yields the nlogous formul for @j C A=RF e now propose proilisti model for the orrespondene errorsD nd develop expliit formuls for its rst two momentsF por simpliityD nd without loss of generlityD let the j i root e situted so tht i C  < j C  @iFeFD the orrespondene error is dened y pose iAF ell the orrespondene error formul of iqution QFTX cij a ~k Rij ~k pij F ri rj k vetting x e the position long the oundry reltive to ~ ik D the orrespondene error is u lolly funtion of xF ith no orrespondene errorD x a HF ine the orrespondene error is lolly olliner with the oundry9s tngentD let ij a cij tk e the projetion of k k ij onto the unit oundry tngent vetorD t D t ~ i F he vetor t is positive pointing from ck uk k k ij is signed quntityD nd cij a ij t F he expeted vlue @menA of ~ i to ~ i F reneD k uk uk k k k i the error in the intervl x P ; i is
+ + +1 +

ij k

i

where @xA is the proility tht the kth sn point from pose j will e loted t xF st is ssumed tht the geometry of the root9s surroundings is not previously knownF hereforeD it is not possile to know priori the proilisti distriution of the orreE spondene errorsD @xAF he resonle ssumption tht @xA hs n priori uniform proility is mdeF ht isD the sn point ~ jk tht is mthed to ~ ik ould lie nywhere u u i i in the intervl ; i with no preferred lotionF rene @xA a I=@i C AF elizing i tht ij @xA a x in the intervl ; i D evlution of iqution QFQH yields k
+ + +

 @xA @xAdx; i  k
ij

@QFQHA

E ij k

i i i a @ Ai @A a i  i C  i i lk a P sinsin i  os k : k sin

+ 2 2 + + 2 2 2

@QFQIA

i xote tht when the inidene ngle is not norml @k Ta WHoAD the men is nonEzeroF rowE

QT everD sine the men is proportionl to sin  D this term is negligile when the mgnitude of  is smllF reneD the orrespondene error n e onsidered to e zeroEmen quntity when  is smll @this holds for the experiments desried in etion QFTAF o ompute the vrine of the orrespondene error @using the zeroEmen ssumptionAD
2

E @ij A2 a k

i

i i i  + C 

x2

dx a

i @ i A C @  A i Q@i C A
+ 3 +

@QFQPA

i i i vetting k a k C k D nd keeping the ove results in mindD the ovrine of the orreE spondene errorD C Pki of iqution QFIHD n e found s CP i k

i i i iA i a @Q@i C @iAA R os k i os k sin k S : i i C os k sin k sin k

+ 3 3 2 + 2

E cij @cij AT a E @ij A2 tk tT k k k k

P

@QFQQA

4 In the case of correspondence error correlations, the likelihood model of Equation 3.11 will no longer take a product form. The form of the likelihood model in this case will depend upon the line tting method.

i xote tht this expression is funtion of the sensor em9s inidene ngleD k F etion QFS disusses how to estimte this quntity from the rnge sn dtF feuse we do not wnt to ssume prior knowledge of the environment9s geometryD the orrespondene errors re onsidered to e independentF his ssumption is onservtive in tht no struture in the environment eyond the immedite geometry of the lol point pirs is ssumedF st would e possile to predit susequent orrespondene errors long wll @or other regulr geometri strutureA given the knowledge tht the susequent orresponding point pirs did indeed ome from the sme extly stright wllF ith proper line tting method @eFgFD see RIAD the orreltions etween orrespondene errors ould e estimted from the line tting method9s unertinty modelF sn generlD knowing tht djent orresponding pirs lie long ommon wll will sigE nintly redue the mgnitude of iqution QFQPD whih in turn will led to lower vrines for most of the points long the wllF sn this seD the orrespondene error vrine eE omes dominted y the unertinty in the wll9s geometryD whih in turn is funtion of the line tting methodF hese eets n t esily within this frmework if desiredD leding to even etter displement estimtes nd tighter estimte ovrinesF roweverD onserE vtive pproh is tken where we do not ssume tht the root9s surrounding geometry is
4

QU priori knownF woreoverD sine the redution in unertinty will only our for points long one line @or other geometri fetureAD in even modestly omplex environmentsD the mount of preision to e gined y using this pproh is unlikely to e worth the omplexity of implementing these more dvned methodsF
3.3.3 Measurement Bias Eects

nge mesurement is is n rtift of some rnge sensing methods @eFgFD see PAF ine is models will strongly depend upon the given rnge sensing methodD it is not possile to give omplete summry of is models for ommon sensing methodsF snstedD generl pproh is onsidered for lulting the eet of is on the displement estimteF o nlyze the is eetD let "ij a "ij C oij D where oij a ~ik Rij ~jk is the totl onstnt ~k R k ~k ~k o o is oset eet t the kth orrespondeneD nd "ij is the previously dened mthing error k @tht ignored the onstnt is termAF snorporting the is osetsD the likelihood funtion tkes the form
v@f"ij gjgij A a ~k
~ " ~ " ~ e 2 (~k ok ) (Pk ) (~k ok ) q ~ P det Pkij k=1

nij

ij

ij T

ij

ij

ij

@QFQRA

~ where Pkij is the ovrine mtrix with is unertinty tken into ountX ~ ij ~ k ~ij T Pk a Qij C Rij Sk Rij ; @QFQSA

ij ~ij ~k b b where Qij a Qij C B P ik nd Sk a Sk C B P jk D with B Pki a E ~ik @~ik AT nd B Pkj a k j @~ j AT F ht isD the ovrine formul is updted to inlude unertinty in the E ~ k bk b is termF o otin these resultsD it is gin ssumed tht the is noise is unorrelted with the rnge mesurement noise nd the orrespondene error @sine vrine in is is typilly funtion of the vriility of the surfe propertiesD rther thn mesurement noiseAF pollowing the derivtions tht led to roposition QFID one n show tht the trnsltion estimte in this se is

pij a Ppp ~

nij  k=1

~ @Pkij A @~ ik Rij ~ jk C oij A u u ~k

1

@QFQTA

QV pormuls nlogous to iqution QFIV n e derived for the orienttion estimte s wellF ij ~k he previous ovrine formuls tke the sme strutureD with Qij nd Sk modied to Qij k ~ij nd Sk @iFeFD to inlude possile is unertinty termsAF glerlyD iqution QFQT shows tht is eets n inuene the displement estimteF roweverD is models n e used to ompenste for is eets in the estimteF
3.4 Selection of Point Correspondences

he fous of this work is to improve displement estimtion vi more urte onsiderE tions of the noise nd unertinty inherent in the estimtion proessF roweverD the disE plement estimtion proess depends upon the ility to suessfully mth orresponding points from rnge sns tken in djent posesF sn order to isolte the enets of the weighted estimtion methodD we use very simple losestEpoint4 rule similr to the one in QUF qiven two sn sets f~ ik g nd f~ jk gD the outliers re removed in the rst stepF hese u u re the points visile in one snD ut not in the other @see QU for detilsAF efter removing the outliersD orrespondenes etween sn point pirs in the two poses re foundF por every point in pose iD we serh for orresponding sn point in pose j tht stises range criterion: he orresponding point must lie within given distneX jj~ i ~ j jj < uk uk dF sf no points in pose j stisfy this riterionD then the point is mrked s hving no orrespondeneF he prmeter d is initilly set t vlue dened y the error in the initil trnsltion estimte @eFgFD the estimted odometry errorAF herefterD to speed onvergeneD d is monotonilly redued to vlue whose order is the mximum point error predited y the noise modelF st is lso possile to estlish point orrespondenes sed on hiEsqured nlysis of point pirs using the detiled sensor noise models lredy omputed in this methodF hough this pproh shows promiseD in experimentl tests we hose to isolte the estimtion enE ets of this workF feuse unweighted snEmthing methods lk the unertinty models to perform hiEsqured sed point orrespondene determintion proessD presented reE sults use the losestEpoint4 method for ll testsD s this leds to the firest omprison proedureF

QW 3.5 Estimating the Incidence Angle he orrespondene error model of etion QFQFP ssumes knowledge of eh sn point9s inE idene ngleF hile ny method of inidene ngle estimtion n e usedD we hve hosen method tht estimtes the lol geometry of the sn points using rough trnsformF he rough trnsform IS is generl pttern detetion tehnique tht is used to determine n estimte of the supporting line segment out pointF he inidene ngle n then e estimted from the ongurtion of the line segmentF sn the generl rough trnsform line nding tehniqueD eh sn point fxk ; yk g is trnsformed into disretized urve in the rough speF he trnsformtion is sed on the prmetriztion of line in polr oordintes with norml distne from the line to the originD dLD nd norml ngleD LX
dL a xk sin@L A C yk os@L A:

@QFQUA

lues of L nd dL re disretized with L P fH; g nd dL P fD; Dg where D is the mximum sensor distne redingF he rough spe is omprised of twoEdimensionl hsh tle of disrete insD where eh in orresponds to single line in the sn point speF por eh sn pointD the ins in rough spe tht orrespond to lines pssing through tht point re inrementedF eks in the rough spe orrespond to lines in the sn dt setF es the ins in the rough spe re inrementedD we mintin history of the ontriuting sn point oordintes in the inD so tht when pek is determined to represent lineD the ontriuting set of points n e reoveredF he inidene ngles n then e estimted for every point in the lineF he lgorithm is only preise up to the level of disretiztion hosen for the line prmE etersF foth omputtionl omplexity nd the memory needed for the hsh tle grow with ner disretiztion so it is importnt to estlish resonle lne etween preision nd omputing resouresF por implementtionD line ngle mesurement preise to the nerest degree is ssumed to e dequte for inidene ngle estimtionF hisretiztion in distne ws set to IH mmD though this hoie of this vlue is less signint s only the orienttion of the t lines is usedF he rough trnsform is not limited only to stright line detetionF st n lso e used to detet nd t simple urves suh s irles nd ellipses nd even ritrry shpes RF he tngent vetors to these urves @nd susequently the inidene ngleA n then esily

RH e estimted from the trnsformF por most indoor environments the line tting method is suient to determine inidene nglesF wore urte line tting methods @eFgFD RI nd referenes thereinA n e used to get more urte estimtes of inidene ngleD ut the extr omputtion is typilly not lned y suiently etter estimtion uryF por points tht re not found to e lustered into lineD no inidene ngle estimte is lultedF hese points re weighted only ording to the omputed mesurement noises suh tht the ovrine of the mthing error t the kth point orrespondene of poses i nd j from iqution QFW eomes
ij R j T i Pk a N Pk C Rij N Pk Rij ;

@QFQVA

where the orrespondene ovrine estimte C Pkij hs een droppedF

3.6 Experiments

his method ws implemented on xomdis PHH moile root equipped with ik vwE PHH lser rnge snnerF his sensor mesures the rnge to points in plne t every hlf degree over IVHEdegree rD s seen in pigure QFPF por the purpose of omprisonD n unweighted lestEsqures sn mthing lgorithm nlogous to tht of vu nd wilios QU ws implemented lsoD nd herefter lled the vF4 foth the weighted nd unweighted estimtion lgorithms used the sme point orrespondene lgorithm so tht the omprison ould firly fous on the reltive merits of oth estimtion shemesF etion QFTFI ompres the roustness nd ury of the lgorithms in four dierent environment geometriesF etion QFTFP ompres results from two longer runsF etion QFTFQ presents the estimted omputtionl osts of the lgorithmsD while etion QFTFR experimentlly explores is ompenstionF ell experiments used the vlues  a H:SoD l a S mmD nd  a IH rdins otined from the ik vwEPHH lser speitionsF
4

3.6.1 Robustness and Accuracy Comparisons

he experiments reported in this setion fous on two spets of estimtion performneX the roustness with respet to errors in the initil displement estimte tht seeds the itertions of the lgorithmY nd the ury of the displement estimtesF e more roust

RI npertured rilX npertured rilX pinl irror in pinl irror in osition @mmA yrienttion @mrdA est eighted v eighted v pig QFQ HFIW IFQQ HFPQ VFV pig QFR IFS QFT HFRQ IFR pig QFS PFS WFV HFSU ITFH pig QFT IFV RFI HFHQQR HFQI le QFIX osition nd orienttion error vlues for trils with no initil perturtionsF erentge of gonverged rilsX gonverged rilsX ISPS ertured everge irror in everge irror in rils gonverged osition @mmA yrienttion @mrdA est eighted v eighted v eighted v pig QFQ WIFH7 TRFW7 HFTQ IFV HFUW VFT pig QFR VPFH7 STFW7 IFV TFH HFTU PFT pig QFS WSFS7 QIFP7 PFS IIFI HFSU ITFH pig QFT USFI7 QFH7 QFI IRFS HFHQWP HFRU le QFPX oustness nd ury omprison sttistis for trils with initil perturE tionsF lgorithm n suessfully reover from wider rnge of errors in the initil displement guessF sn prtieD suh errors in the initil displement estimte ome from lrge odometry errorsD or might rise in the sene of odometry when the initil guess is provided y n open loop estimte of the root9s motion responseF o test for roustnessD eh lgorithm ws run through multiple trils with the sme pir of snsD eh time only perturing the initil displement guessF ome initil guesses were suiently poor tht the lgorithm onverged to n erroneous solutionF en estimte ws deemed suessful when the true mesured displement ly within the Q devition rnge s dened y the lgorithm9s lulted ovrine @the v ovrine ws lulted using the formul given in QTAF he initil displements rnged from H to THH mm t V rdil diretions @every =R rdinsA t inrements of PHH mm in positionD nd rnged from H:T to HFT rdins in orienttionD t inrements of HFHP rdinsF por eh of the PS disrete initilly pertured positionsD TI initilly pertured orienttions were used to generte ISPS unique initil ondition perturtionsF hese perturtions were dded to the true displement to rete initil onditions for the ISPS trils for eh lgorithm nd eh environmentl ondition desried elowF e lso ompre the overll ury of eh lgorithm9s displement mesurementF

RP
A
Robot Pose

Even Scan Points Odd Scan Points Perturbed Initial Displacements

4000 3000 2000 1000

0 1000 (mm)

2000

3000

4000

Closeup : Robot Pose

Unweighted 989 estimates (64.9%) converge to within 3 of true displacement

Weighted 1388 estimates (91.0%) converge to within 3 of the true displacement Initially unperturbed weighted estimate

Initially unperturbed unweighted estimate

True Zero Displacement Unweighted Displacement Estimates Weighted Displacement Estimates Unweighted Covariance (3) Weighted Covariance (3) 4 2 0 (mm) 2 4 6

pigure QFQX eA ixperiments with initil displement perturtions etween sns tken t single poseF fA gloseup of root pose with resultsF he true displements re mesured y hnd with n unertinty of less thn P mm in displement nd HFHHP rdins in orienttionF e rn this roustness nd ury test over four dierent sn pirsF
Single-Pose Test

he rst experiment shown in pigure QFQ tests for roustness nd ury while isolting the eets of the modeling of the point orrespondene error @etion QFQFPAF sn this testD two sns were tken from the ext sme root pose @iFeFD the root ws not moved etween snsAD with one sn omprised only of the even sn points nd the seond sn omprised only of the odd sn pointsF sn this wyD orrespondene errors re rtiilly introdued into the two snsF he two sns nd the initilly pertured positions re shown in pigure QFQeF he

RQ displement estimtes of the suessfully onverged estimtes re shown in pigure QFQfF he results of the two runs with unpertured initil guesses re shown with oldfed mrkersD long with the Q unertinty oundry of these estimtes @shown s dshed ellipsesAF yf the ISPS runs with initil displement perturtions thie weighted lgorithm onverged suessfully in WIFH7 of the ses while the v lgorithm ws suessful in TRFW7 of the sesF he verge error for suessful weighted estimtes ws HFTQ mm nd HFHHHUW rdins while the verge error for suessful v lgorithm estimtes ws IFV mm nd HFHHVT rdinsF he error for the se when the initil displement guess is unpertured is HFIW mm nd HFHHHPQ rdins for the weighted lgorithm nd IFQQ mm nd HFHHVV for the v lgorithmF hough the true displement etween the poses is extly zero @sine the sns were tken t the sme root poseAD due to the evenGodd nture of the sns no two orresponding sn points smple the ext oundry points of the environmentF he eet of this orrespondene error on the v lgorithm n e visulized in the presene of three distint lol minim in pigure QFQfF his multiEmodl result surrounding the vlue is often seen in v lgorithm roustness test resultsF
Two-Pose Test

pigure QFR shows results from initil ondition roustness testing on two sns tken in our l with true position nd orienttion displements of TVQ mm nd HFRTU rdinsF pigure QFRe shows the root poses nd sns under onsidertionD s well s the initil pertured displement guessesF pigure QFRf shows the results otined y strting the lgorithms from the ISPS dierent initil displement perturtionsF he weighted lgorithm suessE fully onverged in VP:H7 of the ses while the v lgorithm ws suessful in ST:W7 of the sesF he verge error for suessful weighted estimtes ws IFV mm nd HFHHHTU rdins while the verge error for suessful v lgorithm estimtes ws TFH mm nd HFHHPT rdinsF he error for the se when the initil displement guess is unpertured is IFS mm nd HFHHHRQ rdins for the weighted lgorithm nd QFT mm nd HFHHIR for the v lgorithmF
Two-Pose Test With IntraScan Changes in the Environment

pigure QFS shows the results of the sme type of testing performed on pir of sns in whih the environment hnged etween snsF xote tht the horizontl doule wll on

RR
A
Pose 2 Pose 1

Pose 1 Scan Points Pose 2 Scan Points Perturbed Initial Displacements

4000 3000 2000 1000

0 1000 (mm)

2000

3000

4000

Closeup : Pose 2

Unweighted 868 estimates (56.9%) converge to within 3 of true displacement

Weighted 1252 estimates (82.0%) converge to within 3 of the true displacement Pose 2

Initially unperturbed unweighted estimate Initially unperturbed weighted estimate

True Pose 2 Displacement Unweighted Displacement Estimates Weighted Displacement Estimates Pose 2 Measurement Covariance (3) Unweighted Covariance (3) Weighted Covariance (3) 280 278 276 274 272 (mm)

270

268

266

264

pigure QFRX eA ixperiments with initil displement perturtions etween sns tken t dierent posesF fA gloseup of pose P with resultsF the lower left side of the gure is tully tle t lmost extly lser heightF he rst sn smpled the wll ehind the tle while the seond sn smpled the front edge of the tle due to smll hnges in oor geometryF he dditionl nery ostrution to the left of the root ws used y person who moved etween the two snsF he rnge points ssoited with these nonErepeting ojets represent PW:P7 of the totl numer of sn pointsF por the ISPS trils with dierent initil displement perturtionsD the weighted lgoE rithm ws suessful in WS:S7 of the sesD while the v lgorithm ws suessful in QI:P7 of the sesF he verge error for suessful weighted estimtes ws PFS mm nd HFHHHSU rdins while the verge error for suessful v lgorithm estimtes ws IIFI

RS
A
Moving Person Pose 1 Pose 2 Table at Laser Height

Pose 1 Scan Points Pose 2 Scan Points Perturbed Initial Displacements 6000 5000 4000 3000 2000 1000 (mm) 0 1000 2000 3000 4000

Closeup : Pose 2

Unweighted 476 estimates (31.2%) converge to within 3 of the true displacement

Weighted 1456 estimates (95.5%) converge to within 3 of the true displacement

Pose 2

True Pose 2 Displacement Unweighted Displacement Estimates Weighted Displacement Estimates Pose 2 Measurement Covariance (3) Unweighted Covariance (3) Weighted Covariance (3) 110 105 100 95 (mm)

Initially unperturbed unweighted estimate

Initially unperturbed weighted estimate

90

85

80

pigure QFSX eA ixperiments with initil displement perturtions in nonEstti environE mentF fA gloseup of pose P with resultsF mm nd HFHIT rdinsF he error for the se when the initil displement guess is unE pertured is PFS mm nd HFHHHSU rdins for the weighted lgorithm nd WFV mm nd HFHIT for the v lgorithmF hese results show tht this method9s emphsis on weighting eh sn point results in superior roustness to the presene of signint numer of nonEorresponding rnge pointsF
Two-Pose Test in a Hallway

pigure QFT shows the results of nlogous testing done in nerly symmetril hllwyF sn perfetly symmetril hllwy with no disernile detils long the wllsD no snE sed lgorithm n eetively orret initil displement errors in the diretion long the hllwy9s min xisF sn this testD single door is open t slight ngle on the left side of the hllwyF he presene of this feture llows for possile sn mthing onvergeneF por the

RT
A
B
Pose 1 Pose 2
Closeup : Pose 2

Unweighted 46 estimates (3.0%) converge to within 3 of true displacement

Pose 2

Initially unperturbed unweighted estimate

Initially unperturbed weighted estimate Weighted 1145 estimates (75.1%) converge to within 3 of true displacement

Pose 1 Scan Points Pose 2 Scan Points Perturbed Initial Displacements 1000 0 1000

True Pose 2 Displacement Unweighted Displacement Estimates Weighted Displacement Estimates Pose 2 Measurement Covariance (3) Unweighted Covariance (3) Weighted Covariance (3) 174 176 178

(mm)

(mm)

180

182

184

pigure QFTX eA ixperiments with initil displement perturtions in hllwy environE mentF fA gloseup of pose P with resultsF set of ISPS initil displement perturtionsD the weighted lgorithm suessfully onverged in US:I7 of the ses while the v lgorithm ws suessful in only Q:H7 of the sesF he verge displement estimte error for the suessful weighted estimtes ws QFI mm nd Q:WP IH rdins while the verge error for suessful v lgorithm estimtes ws IRFS mm nd HFHHHRU rdinsF he error for the se when the initil displement guess is unpertured is IFV mm nd Q:QR IH rdins for the weighted lgorithm nd RFI mm nd HFHHHQI rdins for the v lgorithmF sn eetD the weighted lgorithm etter uses the hllwy9s smll nonEsymmetries to orret the position estimtion long the hllwy xisF his signintly etter performne is primrily due to the pproh of
5 5

RU modeling the orrespondene errorsD whih disounts the ontriutions long the hllwy9s xis @sine there is very low ertinty in tht diretionAF snstedD the smll symmetries re eetively entutedF gonverselyD in the v lgorithm the ontriutions of the nonEsymmetries re eetively lostD resulting in very poor orretion of position errors long the hllwyF he plots of the unertinty ellipses in pigure QFTf lso show how only the weighted lgorithm9s lulted ovrine reets greter unertinty in the diretion prllel to the hllwyD s would e expetedF
3.6.2 Multi-Step Runs

he ove results showed not only the improvement in roustness of the weighted lgorithm over the v lgorithmD ut lso signint improvement in the overll ury of the suessful nl displement estimtesF his improvement in ury is est seen in longer runs with multiple displement estimtes dded end to endF
Long Run with Accurate Odometry

pigure QFU shows QPFVEmeter loop pth onsisting of IHW poses with the nl pose the sme s the strting poseF feuse of the diulty of hnd mesuring eh poseD only the initil nd nl positions re ompredF por eh step the urrent nd previous sns re proessed y eh lgorithm with odometry supplying the initil guessD nd updted displement nd ovrine estimtes re lultedF sn order to mintin sttistil independene in the estimtesD two sns were tken t eh poseD with sn I used to mth with the pose ehind nd sn P used to mth with the pose hedF sn prtil pplitionsD suh dul sn proedure would not e neessryD s ulmn lter ould inorporte the sns while ounting for the orreltion etween suessive displement estimtesF roweverD tht pproh is not used here so tht we n fous diretly on the properties of the displement estimteD nd not worry out the impt of the lter on the resultsF sn order to lose the loopD the seond sn tken t the lst pose is mthed with the rst sn tken t the initil poseF herefore perfet series of displement estimtes dded tip to til would result in extly zero overll displement estimteF por the run shown in pigure QFUD the nl odometry error is IFVIU meters nd HFHT rdinsF he nl v lgorithm error is HFPUI meters nd HFHPI rdins while the nl weighted lgorithm error is HFHRQ meters nd HFHHPW rdinsF he rtio of the nl trnsltion error

RV

pigure QFUX eA e IHWEpose QPFVEmeter loop pthF fA gloseup of nl pth posesD shown the ovrine estimtes of the weighted nd unweighted lgorithmsF to totl pth length is S:SR7 for odometryD H:VP7 for the v lgorithmD nd H:IQI7 for the weighted lgorithmF erhps more importntlyD s shown in pigure QFUfD the nl ovrine lultion for the weighted lgorithm lerly enompsses the true nl pose within the Q oundsD while the ovrine lultion of the v lgorithm does notF

RW

pigure QFVX eA VQEpose PRFPEmeter loopF fA gloseup of nl loop posesF

Long Run with Inaccurate Odometry

he improvement of the weighted lgorithm over the v lgorithm is even more proE nouned in the presene of poor odometry estimtesF pigure QFV shows n tul run where one of the odometry redings ws sustntilly orrupted s the root rolled over door jm when heding into the room in the upper right hnd orner of the plotF his pth is PRFPEmeter loop onsisting of VQ poses with the sns tken nd loops losed s in the previous pthF por the pth shown in pigure QFV the nl odometry error is IFHRH meters nd HFQSR rdinsF he nl v lgorithm error is HFWIW meters nd HFPHH rdins while

SH the nl weighted lgorithm error is HFHIV meters nd HFHIQ rdinsF he rtio of the nl trnsltion error to totl pth length is R:QH7 for odometryD Q:VH7 for the v lgorithmD nd H:HUR7 for the weighted lgorithmF
3.6.3 Comparison of Computational Demands

foth lgorithms were implemented in wtlD nd their omputtionl demnds were nE lyzed using the wtl roler on desktop omputer with entium RD IFVHqrz g with SIPw ewF ithin eh itertionD omputtion is divided etween the point orreE spondene phse @whih usully onsumes the ulk of the omputtionA nd the estimtion phseF he numer of itertions required to reh onvergene lso ets the overll ost of omputtionF sn the IHW steps of run I shown in pigure QFUD the orrespondene method used on oth lgorithms omprises VI:H7 of the totl v lgorithm omputtion time of HFIIP seondsGitertion nd RR:Q7 of the weighted lgorithm omputtion time of HFPHS seE ondsGitertionF por the reltively low initil odometry errors in run ID the v lgorithm onverges in n verge of IPFUV itertions for n verge omputtion time of IFRQ seonds per displement while the weighted lgorithm onverges in n verge of IHFQT itertions with totl verge omputtion time of PFIP seonds per pose displementF por lrger initil odometry errorsD espeilly in orienttionD the dierene in itertions to onvergene inreses to the point where the weighted lgorithm is tully fster thn the v lE gorithmF por the dt shown in pigure QFRD when the orienttion error is greter thn HFP rdins the v lgorithm onverges in n verge of RPFWV itertions for n verge omputtion time of RFVI seonds per displement while the weighted lgorithm onverges in n verge of PPFTH itertions for n verge omputtion time of RFTQ seonds per disE plementF sn summryD experiments show tht in rel world indoor environmentsD the weighted method provides signintly greter estimtion ury nd roustness s ompred to n unweighted pproh without signint inrese in omputtionl ostF glerlyD the omputtionl demnds in the estimtion phse re lrger for the weighted lgorithm @s ompred to n unweighted lgorithmAF roweverD sine the omputtions required y the estimtion prt of the lgorithm ount for only smll portion of eh itertionD nd the lgorithm often onverges in fewer itertions ompred to the v lgorithmD the totl

SI
5 1.5m 3m 4.2m 0

10

15 bias (mm)

20

25

30

35

40

45

10

20

30 40 50 angle (degrees from normal)

60

70

80

pigure QFWX he is t funtionF run time is reduedF

3.6.4 Experiments with Bias Compensation

por ompletenessD we lso implemented the is ompenstion sheme of etion QFQFQF sn order to implement this shemeD we experimentlly determined the lser9s rnge is in ontrolled lortory settingD nd t funtionl reltionship to the experimentl dtF por experimentsD white pper trget ws pled t known distne from the sensorF he enter em of the lser ws ligned so s to e norml to the xis of rottion of the trgetF e totl of IHH rnge mesurements were reorded for every IH degrees of rottion up to VH degrees from the normlF his proess ws repeted for nominl rnges of pproximtely IFS mD Q mD nd RFP mF efF SV provides more detiled hrteriztion of this spei lserF he dt provided there ould e used to uild more detiled model s ompred to the one given elowF his experiment showed tht the is for this prtiulr lser sensor is funtion of oth distne nd inidene ngleF e funtion ws t to these dtD whih ws then employed to determine the isD br @in mmAD in the mesurement given the reported distneD r @in mmAD from the lser sensor nd the ngle from normlD  @in rdinsAD from the rough trnsformF pigure QFW shows oth the dt olleted nd the tting funtionF he is funtion is given

SP
br @r; A a IR C H:HHRr H:HQSe4:9 :

@QFQWA

hen this is model ws inorported in the vw estimtion proessD the resulting position estimtes were lmost unhngedF yver the PIFVEmeterD eightEstep pth desried in RPD the inorportion of the is term resulted in n improvement of only IFV mm or HFHHVP7 in the nl position estimteF here re two resons for suh smll ontriution from the is termF pirstD s n e seen in pigure QFWD this lser9s is is quite smll nd reltively onstnt @$I mA for ngles up to TH degrees from normlF his exellent ehvior is ertinly due in prt to preEproessing tht ours inside the sensor itselfF wost of the orresponding points proessed y the vw lgorithm re reorded t ngles within the TH rngeF et lrger inidene nglesD rnge points re usully sprsely distriuted on surfes fr from the sensor nd re usully rejeted y the mthing lgorithm sine they nnot e pired with the required level of ondeneF iven if these points re inludedD their ssoited mthing ovrine is lrge enough to mke their eetive ontriution negligileF woreoverD symmetries in the environment result in mutul nelltion of the is eet introdued y points found in opposite diretionsF xevertheless similr proess for estimting the is n e used nd n provide improved ury in the se of lowerEqulity distne mesuring sensors tht experiene signint isF
3.7 Conclusions

his hpter introdued new method for estimting root displement sed on dense rnge mesurementsF sn prtiulrD the eets of dierent error nd noise soures on the onvergene nd ury properties of these motion from struture lgorithms were invesE tigtedF ixperiments showed tht reful ttention to the detils of error modeling n signintly enhne overll displement nd ovrine estimtion uryF he rst prt of the hpter gve generl formultion of the displement estimtion prolem using weighted point pir orrespondenesF e generl solution to the estimtion prolem nd formuls for the ovrine of the displement estimte were then derivedF he pplition of these results then depends upon expliit error modelsD nd generl models for rnge mesurement noiseD is errorD nd orrespondene error were presentedF elthough prts of this nlysis were minly imed t plnr lser rnge sensorsD the methods n likely

SQ e extended to lgorithms for nonEplnr lser snners QPD IVD where detiled unertinty modeling hs not een onsideredD nd other rnge sensors suh s stereo mersD rdrD ultrsoundD etF hese tehniques should lso e useful for methods tht use oth plnr lser rnge nders nd mers to estimte threeEdimensionl motion prmeters SPD RVF he speis of this nlysis must e modied to inorporte the pproprite errorGnoise models for eh prtiulr sensorF he urte displement estimtes orded y this method n e fused with odomE etry estimtes RS to provide etter root loliztion pilityF imilrlyD the improved displement estimtion orded y this method should in the future led to more urte mp mking nd loliztion proeduresF

SR

Chapter 4
Optim-Bug
his ghpter gives desription nd onvergene results for the yptimEfug lgorithmF vike ngent fugD yptimEfug is omplete nd orret plnner tht ssumes perfet ded rekoningF hetiled simultions re not presentedD euse ides from yptimEfug re primrily intended to help introdue the nertin fug lgorithm in ghpter TF etion RFI denes the terminology used in the exmples nd in the proof of ompletenessF etion RFP summrizes the yptimEfug lgorithmD nd etions RFQ nd RFR provide more detils nd the proof of onvergene nd ompletenessF he ngent fug lgorithm hooses the locally optimal pth while it is in motion to gol @wtgA modeF he pth is lolly optiml @insted of glolly optimlA euse the root does not hve omplete informtion out the environmentF ell tht the seond mode of opertion for ngent fug is oundry following @fpA modeF sf the ost of pth is dened s the pth lengthD then the pths generted while in wtq mode re the ext optiml solutions given present knowledge out the world nd ignoring knowledge of previous ostlesF roweverD the pths hosen while in fp mode re not optiml solutionsF xow onsider n lgorithm where the root opertes in single modeX it lwys follows the lolly optiml pth to the golD regrdless of whether or not tht pth moves towrds or temporrily wy from the golF he ost of pth is dened s the pth lengthF his lgorithm is lled yptimEfugF4
4.1 Setup and Denitions

his setion introdues some of the onepts nd terminology tht will e used lter in yptimEfug9s proof of onvergeneF vet g denote the conguration space of the root|the

SS set of ll possile ongurtions the root n tke PSF st is ssumed tht the environment is populted with nite numer of ompt ostles whose oundries re smoothF he freespace is dened s the omplement of the ostles9 pointsX
p a g n i yi;

@RFIA

where the yi re the physil ostlesF p is the losure of freespeD whih is the freespe plus the ostle oundriesF vet xk denote the root9s ongurtion t time kX
xk a x@tk A:

@RFPA

st is lso ssumed the root is equipped with n omnidiretionl rnge sensor with mxE imum sensing rnge of RF vet v@xk A e the current visibility set t ongurtion xk X
v@xk A a q j q P p Y kq xk k < RY q@I tA C xk t P p Vt P H; I :

@RFQA

his is the region ontined in freespe entered t xk D nd ounded y the root9s sensing rnge RF he oundry of the urrent visiility set onsists of irulr rs with rdius R nd the sensed ostle oundries from root lotion xk F pigure RFI shows sequene of hypothetil steps of the yptimEfug lgorithmF et eh root lotionD the visiility set is leledF
v3 O1 x2 v4 xR v5 O3 xg

xs

O2 v2

O4

v1

pigure RFIX he current visibility setsD v@xk AD shown t eh lotion of fourEstep pthF et x D ostle y is blocking obstacleF
2 2

e blocking obstacle is dened s n ostle tht the root enounters tht loks the

ST strightEline pth to the golF ht isD if the ondition

yi v@xk A Ta Y; xg @I tA C xk t P yi

@RFRA

is true for some t P H; I t step kD then ostle yi is loking ostleF sn pigure RFID ostle y is loking ostle from root lotion x euse it lies etween the root nd the golF he total visibility setD Vtot@xk AD is the union of ll the v@xk A for ll root ongurtions up to time kX k Vtot @xk A a v@xi A: @RFSA
2 2

pigure RFP shows the totl visiility set for the exmple of pigure RFI @whih onsists of ve root positionsAF he totl visiility setD Vtot@xk AD ontins ll of the environment tht the root hs seen up to time kD nd therefore hs memory ofF
xg Vtot

i=1

pigure RFPX he total visibility setD VtotD for the sme fourEstep hypothetil exmple of pigure RFIF
4.2 Optim-Bug Overview

here re two min dierenes etween ngent fug nd yptimEfugF he rst dierene is tht yptimEfug hs only one mode|tht of nding the shortest pth to the gol given its urrent knowledge of the worldF sn the version presented hereD yptimEfug requires more memory thn ngent fug doesD euse it must rememer ll ostles seen up to the urrent ongurtionF

SU o llow this single mode of opertionD it is neessry for yptimEfug to uild up mp of the environment s it moves longD rememering the lotion of ny portions of ostles tht it hs seenF his memory is needed for two resonsF pirstD it removes the neessity for oundry following modeF ngent fug hs oundry following mode in order to eliminte the need for memory of ostles it hs seenF eondD the memory llows the root to detet when the gol is ompletely enlosed inside n ostleD nd thus delre filureF pigure RFI depits short hypothetil sequene of exeuted steps of yptimEfugD ilE lustrting the required memory of ostle lotions nd geometriesF he root strts t lotion xsD while the gol is t xg F et the point shown in the gureD it hs tken four steps to lotion xR F huring these onseutive stepsD it hs seen nd rememered the drkened portion of the ostlesF he unknown ostle prts re shown in lighter olorF st is ssumed tht the ostles lok sensing of other ostlesD so the known ostles re modeled s thin wllsF et the eginning of eh new stepD the known portions of the ostles re used s onstrints in the optimiztion proessF es long s the gol is elieved to e rehleD the shortest length pth is omputed t eh stepF yne the shortest pth is foundD the root follows this pth until it rehes the edge of its totl visiility setF sn the exmpleD this would e the oundry of the overlpping gry dissF yne it hs stepped to the oundry of the totl visiility setD the root tkes its next view of the environmentD therey dding the newly viewed re to its mpF his proess ontinues until the root rrives t the golD or the gol is determined to e unrehleF sn summryD one step of the yptimE fug lgorithm proeeds s followsD ssuming the root strts t xk nd Vtot@xk A hs een omputedX

hile the gol is still rehleX IF gompute the shortest length pth to the gol sed on Vtot@xk A @iFeFD tking into ount urrently known nd previously seen ostlesAF PF pollow the pth to the oundry of the totl visiility set @position xk D or to the gol if it is within the urrent visiility setF sf the root hs rehed the golD terminte with suessY ytherwiseD ompute the new visiility setD v@xk AF QF edd the newly viewed regionD v@xk AD to the mpX Vtot@xk A a Vtot@xk A v@xk AF
+1 +1 +1 +1 +1

SV RF sf there is no pth tht rehes the gol without interseting n ostleD termiE nte with filureF his mens tht the gol is inside n ostleF ith perfet informtionD iFeFD no odometry nd sensing errorsD ngent fug is likely more prtil lgorithm thn yptimEfugF st requires very little memoryD nd in mny ses the pths generted y ngent fug nd yptimEfug will e quite similrF es shown elowD in the se of nonEonvex ostlesD yptimEfug n temporrily osillte k nd forth on n ostle oundryD while ngent fug hooses more uniform pthF yptimEfug9s memory requirements re greterD s the root must rememer the lotion of every ostle it hs seenF roweverD in omplex environmentsD yptimEfug my hve some dvntgesF st is lso likely tht the extension of yptimEfug to QEh is reltively strightforwrdD wheres the QEh version of ngent fug is quite umersomeF ell the ssumption of n omnidiretionl rnge sensorF sn most relisti situtionsD suh sensor would sense only t disrete ngles round the rootF sf the ngulr disE retiztion is smll enough nd ll ostles re lrge reltive to this ngulr seprtionD the sensor ould e onsidered to e ontinuousF roweverD it my e desirle to implement lol ostle voidne sheme if the ngulr disretiztion is lrgeD or if ostles re smll reltive to the ngulr seprtionF sn these situtions the sensor ould miss deteting n ostleF feuse yptimEfug lwys follows the pth to the oundry of VtotD lol ostle voidne sheme would mitigte these issuesF his disussion of yptimEfug will e used to help motivte the struture of nertin fug in ghpter TF nertin fug uses n optimiztion frmework tht is dierent from ngent fugF he ost funtion used in nertin fug is dierent thn tht of yptimEfugD ut some of the generl issues re the smeF he root lwys seeks fesile pths tht re optiml in the ontext of known informtionF es long s fesile pth existsD the root will ontinue to ttempt to reh the golF sf nd when the root nds tht no fesile pths existD it n onlude tht the gol is unrehle from its urrent stte nd delre filureF et high levelD the proof for ngent fug uses reltively simple vypunovEsed methodF huring the motion to gol modeD the root lwys dereses the distne from itself to the golF sn oundry following @fpA modeD the root is llowed to inrese this distne temporrilyD ut it nnot swith out of fp mode until it hs rehed point

SW nerer to the gol thn the losest it ould hve een when it strted fp modeF husD the root is lwys mking progress towrds the gol @in wtq modeA with the exeption of nite numer of swithes into fp modeF ine the root only leves fp mode when it is gin loser to the golD the trend is tht the root is lwys moving loser to the golF gonsequentlyD the distne from the root to the gol onverges to zeroD sine there re only nite numer of ostlesF yf ourseD if the root stys in fp mode long enough to ompletely irumnvigte n ostleD then the gol is unrehle nd the lgorithm termintesF he proof methodology for yptimEfug is slightly dierentF sn the following setionD it is shown tht the shortest pths follow lines tht re either stright lines or re tngent to the urrently known ostlesF hen it is shown tht there re nite numer of suh pthsF pinllyD the lgorithm is strutured so tht in nite numer of stepsD the root must reh strightEline pth to the golD or onlude tht the gol is unrehleF
4.3 Shortest Path Properties

pirstD let us onsider the properties of optiml pthsD euse these properties re needed for the proof of onvergene nd ompletenessF vet @tA e smooth pth in p F felowD the neessry onditions for the shortest pth in freespe ounded y smooth urves re sttedF hortest pths onsist of segments tht re eitherX IF stright line from the root to the gol in freespeD or PF tngent to the onvex hull of ostlesF sn generl the formul for the length of smooth pth @tA X a; b 3 Rk @using the iuliden normA is given y b l@A a k@tAkdt: @RFTA eppendix g gives detils on the proof of these onditionsF sn dditionD suh fetures re well estlished in the visiility grph litertureF
a

4.4 Proof of Completeness

TH

vet goal-tangent line e strightEline segment tht psses through the gol nd is tngent to n ostle oundryF e tangent point is point on n ostle where the tngent line is tngent to the ostleF e true goal-tangent is dened to e golEtngent tht lies entirely in the losure of freespeF sf golEtngent is not true golEtngentD it is dened to e false goal-tangentF en obstacle endpoint is point where n ostle oundry intersets the oundry of the totl visiility setF pigure RFQ shows n exmple ostle ongurtion nd its ssoited golEtngent linesD tngent pointsD nd ostle endpointsF
Obstacle endpoint Tangent Point

T1 T2 xR T3 xg

pigure RFQX e hypothetil exmple showing ostle endpointsD tngent pointsD nd their ssoited golEtngent linesF es desried in the lgorithm overviewD the root is ommnded to follow the optiml pth to the oundry of the totl visiility setD VtotD t eh stepF sf the strightEline pth to the gol is lerD the root will follow it to the oundry of VtotF hen the root enounters loking ostleD the optiml pth will e tngent to the known ostle oundriesD iFeFD the portion of the ostles tht lie in Vtot@xk AF he pth plnned from xk will lwys pss through n ostle endpoint of the urrent loking ostle euse of the properties of the shortest pthsF woreoverD the root will lwys move toD nd end its urrent itertion tD one of the ostle endpoints in the presene of loking ostleF sn prtieD it is ssumed tht the root will move  wy from the ostle oundry t the ostle endpointF st is lso ssumed tht the root9s sensing rnge is greter thn this distneD iFeFD R > F

TI Axiom 4.1. et n ostle endpointD the ostle oundry is trnsverse to the oundry of VtotF sf n ostle oundry were not trnsverse to the oundry of Vtot t n ostle endpointD it would imply tht the ostle oundry point ws still within the oundry of Vtot D with its lotion further long @Vtot F sf tht were the seD then the root would see tht prt of the oundryD nd the ostle endpoint would e in dierent lotionF e nonEgeneri se n our if the ostle oundry hs n inetion point tht lies on the oundry of VtotF eny smll perturtion of the root position or the ostle lotion will eliminte this seD so it is ssumed tht it will not ourF
Lemma 4.1. The total visibility set, Vtot , grows with each robot step.

essume the root hs moved to xk from xk F vet sk e the intersetion of the urrent visiility set t xk nd the previous totl visiility setX
Proof.
+1 +1 +1

sk a Vtot@xk A v@xk A:
+1 +1 +1

@RFUA

sn other wordsD sk is the prt of the root9s environment known t step k tht n still e seen t step k C IF vet hk e dened s
+1

hk a v@xk A n sk :
+1 +1 +1 +1 +1

@RFVA

for this vemm to e trueF fy denition of the yptimEfug lgorithmD the root lwys follows the shortest pth to the edge of the totl visiility set t eh stepF he properties of the shortest pth ditte tht the next root ongurtionD xk D will lie either in freespe t the oundry of VtotD or t n endpoint of sensed ostle oundryF ell the ssumption tht in the se of loking ostleD the root mintins some smll distneD D wy from the ostle oundryF st is lso ssumed tht the root sensing rngeD RD is greter thn F he oundry of Vtot onsists of irulr rs of rdius RF sf xk lies in p D then the new visiility setD v@xk A must ontin some newly seen reF husD the root sees previously unseen prt of the environmentD nd hk Ta YF
+1 +1 +1 +1

hk is the new4 prt of the world seen y the root t step k CIF hk must e nonEempty

TP
@ Oi

Vtot(xk )

 v(xk+1) xk+1


Newly viewed area

pigure RFRX sllustrtion for vemm RFI showing tht the totl visiility setD Vtot grows t eh stepF sf xk lies  wy from n ostle endpointD the root will still see some new region of the environmentD nd hk Ta YF ine R > D W some  > H suh tht ll of rdius  entered t the ostle endpoint will lie inside v@xk AF feuse the ostle oundry is trnsverse to the oundry of Vtot@xk AD the ll must ontin some newly seen reF pigure RFR depits this grphillyF husD the totl visiility set grows with eh root stepF
+1 +1 +1

hen the root enounters loking ostleD the properties of the optiml pth diE tte tht the pth will e tngent to the onvex hull of ostlesF he root is nvigting4 round loking ostle until it rehes point where the optiml pth is no longer tngent to tht prtiulr ostle oundryF his will hppen if the root rehes the gol or rehes golEtngentF
Lemma 4.2. While navigating around a blocking obstacle, the robot's next step will always
take it to an endpoint of a known obstacle boundary segment.

he properties of the optiml pth @ee eppendix gA imply tht the optiml pth will e tngent to the onvex hull of the known ostle oundries in Vtot@xk AD where xk is the urrent root ongurtionF
Proof.

TQ he yptimEfug lgorithm dittes tht the root9s next step will follow the optiml pth to the edge of Vtot@xk AF iither the pth will e stright line towrds the golD in whih se there is no loking ostleD or the pth must e tngent to the loking ostleF unown ostle oundry endpoints lie on the edge of Vtot@xk AF hereforeD when nvigting round loking ostleD the root9s next step will tke it to n endpoint of known ostle oundry segmentF yne the root hs stepped to one of the ostle endpointsD the shortest pth will ontinue to follow the ostle oundry or its onvex hull until it rehes tngent pointF et tngent pointD the optiml pth deprts the ostle oundry nd follows the sE soited golEtngent lineF et tht pointD the root is no longer nvigting round tht prtiulr loking ostleD lthough it my reEenounter it lterF he golEtngent line is strightEline pth to the gol y denitionD so the yle strts ll over ginF he root will follow the golEtngent line until it eitherX

rehes the golD enounters nother loking ostleD reEenounters lolly nonEonvex portion of the sme ostleF

vet @ yi denote the oundry of ostle iF vet @ y denote the oundry of ll ostlesD iFeFD @ y a @ yi : @RFWA vet the oundry of yi e prmeterized y s P RF he prmeterD sD is similr to n rE length prmeter tht denes the distne long the ostle oundry from some strting point where s a HF roweverD s does not need to e n rElength prmeterF he prmeterD sD inreses in lokwise diretion round the ostleF he tul lotion of s a H on the oundry is of no onsequeneF vet xi@sA denote the oundry point on @ yi t distne sF vet S i @a; bA denote ontinuous oundry segment of ostle yi from s a a to s a bD iFeFD S i @a; bA a fsjx@sA P @ yi ; sa s sb g : @RFIHA
i

TR i vet fk denote the set of ontinuous intervls of the oundry of ostle yi seen up to step kD iFeFD i fk a S i @a; bA ; S i @c; dA ; : : : Sji @; A ; @RFIIA
1 2

where j is the numer of segments of yi seen up to step kF

Lemma 4.3. Assume the robot has encountered yi as a blocking obstacle at step p. While
navigating around

yi ,

the robot will always see at least one new portion of the blocking

obstacle boundary that is contiguous with a previously known segment.

hen the root rst enounters yi t step pD it will see t lest one oundry segE mentD though it my see moreF sf more thn one segment is seenD without loss of generlityD let S i @;  A e the segment whose endpoint is ontined in the optiml pth omputed t xp F he properties of the optiml pth ditte tht the next root step will pss through either xi@A or xi@ AF he properties of the yptimEfug lgorithm ditte tht while nviE gting round loking ostleD the next root step will end t one of these points @see vemm RFPAF ithout loss of generlityD ssume tht the root moves to xi@A t step p CIF ee pigure RFS for grphil interprettionF feuse it is known ostle endpointD the point xi@A is neessrily on the oundry of the totl visiility setF iven though the root will move to point  wy from xi@AD the root will see new segment of the loking ostle oundry euse R >  nd euse the ostle oundry is trnsverse to @VtotF ithout loss of generlityD let this new segment e S i @; AF feuse the root is t xi@AD the point s a  on @ yi must e within the urrent visiility set nd lie on the new segmentD iFeFD
Proof.
1 2

 <  < :
2 1

@RFIPA

husD the new segment S i @; A must overlp the old segmentD S i @;  A @see pigure RFSAF et step p CPD the root ould move to either xi@ A or xi@A @ee pigure RFSAF egrdless of whih of these two points the optiml pth psses throughD the sme nlysis pplies s the root ontinues to nvigte round the loking ostleF hereforeD the root will lwys see t lest one new loking ostle oundry segment tht is ontiguous with previously known segmentF sn prtiulrD the new segment will

TS e ontiguous with the urrent loking segmentF feuse the s a H point on the ostle oundry n e ritrrily ssignedD this point n lwys e hosen suh tht the ove inequlities holdF

 xp xp+1   s

pigure RFSX et every step while nvigting round loking ostleD the root lwys sees new segment of the ostle oundry tht is ontiguous with previously known segmentF st my see dditionl oundry segments tht re not ontiguousF
Proposition 4.4. From the time that the robot rst encounters a blocking obstacle, it will
map a contiguous segment of the boundary until a goal-tangent point is contained in the blocking segment, assuming there exists a goal-tangent point on the blocking obstacle. The goal-tangent will be reached in a nite number of steps.

he optiml pths will e tngent to the known ostle oundries nd the golF hereforeD the optiml pth will deprt from the loking ostle9s oundry t golE tngent pointF yne golEtngent point lies in known oundry segmentD the root will no longer keep moving to the endpoints of known ostle oundriesF snstedD it will follow the golEtngent pthF fy vemm RFQD the root will lwys see new segment of the loking ostle oundE ry t eh step tht is ontiguous with the previously explored oundry segmentF
Proof.

TT husD if it existsD golEtngent point on the loking ostle oundry will eventully e ontined in the newly mpped oundry segmentF he root my still ontinue to see ontinuous portion of the loking ostle s it follows the golEtngentD ut it is not required for the lgorithmF es shown elowD golEtngent will not exist only when the gol is not rehleF feuse the new oundry segment the root sees is of nite sizeD nd euse the ostle oundries hve nite sizeD the root will reh the golEtngent in nite numer of stepsF sf the ostle is lrger thn the root9s sensing rngeD the root my osillte k nd forth long the ostle oundry s it ontinully nds nd follows the shortest pthF pigure RFT shows hypothetil sequene of steps where the root osilltes k nd forth long n ostle oundry until it rehes tngent pointF he proess of moving long the ostle until tngent point is rehed will tke nite mount of timeF et eh stepD s the root moves to n ostle endpointD it lerns nd mps new portion of the ostle oundryF ell tht s the root follows golEtngent pth tht emntes from the oundry of the i loking ostle it will enounter one of the following three onditionsX IF he root rehes the golF
th

PF he root enounters new loking ostleF QF he root reEenounters the sme loking ostle t new unexplored oundry segmentF his will our if the i loking ostle is nonEonvexF sn some wys situtions P nd Q re treted similrlyF st my not initilly e possile for the root to know whether the newly enountered loking ostle is indeed new ostle or previously enountered one of whih it hs prtil knowledgeF he newly enountered loking ostle ould e portion of nonEonvex ostle tht the root hs lredy seenD s shown in pigure RFUF he root is rst loked y the ostle t lotion xsF he shortest pth tkes the root long the oundry until it rehes golEtngent T F he root ontinues long T until it is loked ginF et this point the root will serh k nd forth long the ostle oundry until it rehes T F
th 2 2 1

Proposition 4.5. Goal-tangents are not revisited.

TU
Step 1 Step 5

xg

xg

Step 2

Step 6

xg

xg

Step 3

Step 7

xg

xg

Step 4

... Step 11

xg

xg

pigure RFTX e hypothetil sequene of steps showing how yptimEfug n osillte k nd forth long loking ostleF his lso illustrtes how ontinuous piee of the oundry segment is mpped outF

TV
Robot path

T1 xR xs T2 xg

T3

pigure RFUX e hypothetil exmple where the root is loked y the sme ostle twieF ortions of the ostle in the root9s mp re denoted y drker olorF fy denitionD golEtngent will pss from point tngent to n ostle oundry to the golF sf the golEtngent the root is following is not lokedD the root will reh the gol long tht golEtngent nd terminte with suessF sf the golEtngent tht the root is following is lokedD the root will enounter loking ostle long tht golEtngent lineF yne the root sees the segment of the loking ostleD S i @a; bA nd ples it in its mpD no pth to the gol n lie long tht golEtngent lineD sine it would interset the ostle segmentF husD the root will never follow tht prtiulr golEtngent ginF
Proof.

Lemma 4.6.
Proof.

W only a nite number of goal-tangent lines.

th

vet i@sA e mp from the i ostle oundry length prmeter s to the ngle sutended y the line from xg to xi@sAD with respet to the xExis of xed glol referene frmeF feuse @ yi is ssumed to e smoothD i@sA is smoothF qolEtngent points for yi will e lol extrem of the funtion i@sAF feuse i@sA is smoothD nd s lies in ounded intervlD i@sA will hve nite numer of lol extremD nd eh ostle will hve nite numer of golEtngentsF feuse it is ssumed tht there re nite numer of ostles in the environmentD there re nite numer of golEtngent linesF

TW
Proposition 4.7. If the goal is not located inside an obstacle, i.e., the goal is reachable,
the algorithm will nd a true goal-tangent after a nite amount of time. Proof.

vet q e the set of golEtngentsD iFeFD

q a fgig ; i a I; : : : ; N;
th true

@RFIQA

where gi is the i golEtngentD nd N is the numer of golEtngent linesF q n e divided into two exlusive sets|the set of true golEtngentsD q D nd the set of flse golEtngentsD q X
false

q q

true

false

a a

true gk ; k a I; : : : ; Ntrue ;

false gk ; k a I; : : : ; Nfalse :

@RFIRA @RFISA

fy denition of the true nd flse golEtngentsD

q
true

false

a Y:
true false

@RFITA

vemm RFT implies tht q hs nite numer of elementsD so q nd q must lso hve nite numer of elementsF hen the root strts outD it my or my not e on golEtngentF sf it isD nd it is on g P q D it will reh the gol on tht prtiulr golEtngentF sf it strts out on g P q D then y roposition RFR it will reh new golEtngent fter nite numer of stepsF his new golEtngent ould e in q or q D nd the proess will ontinueF sf the root does not strt out on golEtngentD roposition RFR implies tht it will nd one fter nite numer of stepsF q is nite setF he lgorithm will ontinully nd new elements in q F fy ropoE sition RFSD golEtngents re not revisitedF iither the root will nd g P q D or it must exhust ll elements in q D therey eventully rehing g P q fter nite numer of stepsF
true false true false false true false true

yne golEtngent line hs een explored nd found to e lokedD the root will never follow tht golEtngent ginF sf the gol is rehleD t lest one of the golEtngent lines must e wholly ontined within the freespeF feuse there re nite numer of golE

UH tngents nd the root spends nite mount of time to trnsition from loking ostle to golEtngentD yptimEfug will reh the gol in nite mount of time if the gol is rehleF sn the worst seD the root will explore ll of the golEtngent linesF sf the gol is not rehleD the root will detet tht the gol is enlosed in n ostleF ell tht yptimEfug ssumes perfet odometry nd sensingD so the root9s mp tht is uilt during its explortion will lso e perfetF sn the filure seD it is not neessrily true tht the root will explore ll of the golEtngent lines efore delring filureF Proposition 4.8. If the goal is inside an obstacle yi , i.e., the goal is not reachable, then
the robot will circumnavigate Proof.

yi in a nite number of steps, indicating failure. words, no boundary segments of yi will be unseen.

In other

essume tht the gol is inside ostle yiF here my or my not exist golEtngents depending on the shpe of the ostleF sf yi does not hve ny golEtngent lines @eFgFD it is onvexAD interprettion of roposiE tion RFR mens tht the root will ompletely irumnvigte the ostle without nding ny gol tngent linesF purthermoreD this mens tht it will mp out the entire oundry of the ostle s one ontinuous segmentF sf yi does hve golEtngent linesD y roposition RFR the root will mp out ontinuous segment until it rehes one of the golEtngents @eFgFD if the gol were inside the ostle of pigure RFUD there ould still e golEtngent linesAF feuse the gol is inside the ostleD ny golEtngent @even those tht re tngent to other ostlesA will neessrily e lokedF sn this seD the oundry of yi will not e mpped out in one ontiguous segmentF ine the lgorithm lwys nds new ontiguous piee of ostle oundry t eh step @vemm RFQAD it will determine tht the disonneted segments re ll prt of the sme ostle in nite numer of stepsF
Theorem 4.9. Optim-Bug is complete.

eording to roposition RFVD if the gol is not rehle the root will determine this nd delre filure in nite numer of stepsF sf the gol is rehleD the root will nd true golEtngent in nite numer of steps y roposition RFUF eording to vemm RFTD there re only nite numer of golEtngent linesD nd one golEtngent line is found to e loked it is never revisited @roposition RFSAF
Proof.

UI husD yptimEfug will determine if the gol is rehle or not nd terminte in nite mount of timeF sn some senseD there is list of golEtngents tht the root n hek o one y one s it nds them to e lokedF sn the worst seD it will end up heking ll of the loked golEtngents efore it nds one of the golEtngents tht rehes the golF
4.5 Conclusion

his hpter presented the yptimEfug lgorithmD omplete nd orret plnnerF yptimE fug is sed on nding nd following the shortest pth in the environmentF yptimEfug ssumes point root with sensor tht hs nite rngeF vike ngent fugD yptimE fug requires perfet dedErekoningF yptimEfug requires the root to keep memory of ostles it hs seenF yptimEfug my prove to extend well to higher dimensionsF here is QEh version of ngent fug PID ut the proof of ompleteness is nonEintuitiveF yne possiility for improvement is in eliminting the requirement tht yptimEfug reE memer the lotion of every ostle it hs seenF sf one pled ound on the size of ostles in the root9s environmentD it seems tht n ostle ould e removed from the root9s memory one the root hd moved ertin distne wy from itF sf the root ould e done4 with n ostle nd gurntee tht it would never see tht ostle ginD it ould redue the overll memory requirements of the lgorithmF ghpter T presents the nertin fug lgorithmD whih does not require the root to hve perfet dedErekoningF st uses n optimiztion frmework tht is similr to yptimE fug9sD ut uses dierent ost funtionF ome of the nlysis performed in this hpter will help motivte similr nlyses of the nertin fug lgorithmF

UP

Chapter 5
Path Optimization
his hpter desries n optimiztion proedure used to nd the pth tht root n follow in order to minimize its positionl unertinty t the golF he optimal path is dened s the pth tht hs the lest root pose unertinty t the golF his proedure will nd the entire pth from the root9s urrent lotion to the gol while voiding ostlesF sn ghpter TD this optimiztion method is modied to t into sensorEsed motion plnning lgorithmF elthough s will tlk out the optimal pthD it is possile tht the pth generted y the lgorithm is only lol optimlD s opposed to the glolly optimlD solutionF he presene of ostles in the environmentD nd the nonElinerity of the ost funtion mke it prtilly diult to nd the glolly optiml solutionF
5.1 The Localization Framework

his setion desries the generl loliztion frmework tht is ssumed for this plnning methodF s use stndrd ulmn ltering pproh s desried in etion PFPFP to trk the ovrine of the root stte nd to perform loliztionF he ovrine of the lter will ply entrl role in the pth plnning methodF es in etion PFPFPD ssume tht the root is given the lotion of N lndmrksD s well s the ovrines of these lndmrk lotion estimtesF he environment is populted with nite numer of ostlesF he root9s tsk is to pln ollisionEfree pth from its urrent position to the gol lotionD xg D point speied in the glol referene frmeF his setup is shown in pigure SFIF e rief summry of the setup from etion PFPFPX he stte vetor x ontins the

L1

UQ

L2 xR xg L3

L4

pigure SFIX he environment setup ssumed for the pth plnning proessF he root strts t lotion xR nd is instruted to trvel to xg F L D L D L D nd L re lndmrks tht the root hs knowledge ofF he irles re ostles tht the root must voidF
1 2 3 4

positions of oth the root nd ll of the lndmrksD iFeFD

x a xR xL1
h

xLN ;

iT

@SFIA

where xR is the grtesin position of the root nd xLi is the grtesin position of the ith lndmrkD iFeFD P Q P Q xLi S xr S xR a R nd xLi a R : @SFPA
yr yLi

por the rnge nd ering oservtion model presented previously @iqution @PFPUAAD the mesurement of lndmrk i is
zi @k C IA

xLi @k C IA xR @k C IA C ni @k C IA;

@SFQA

UR where ni@k C IA is zeroEmen white qussin noise proess with ovrine

Ri @k C IA a E ni @k C IAni @k C IAT :

@SFRA

he mesurement noise need not e the sme for eh lndmrkF ngeEdependent vritions re llowed in the noise model in order to ommodte mny relisti sensing situtions @see iqution @SFITA elowAF elthough ll exmples presented use the rnge nd ering mesurementsD this formultion llows for ny type of mesurementsF et eh mesurement updteD the root tkes mesurement of every lndmrkD so the entire mesurement vetor nd mesurement ovrine re given y a ; R@k C IA a dig@R ; R ; :::; RN A :
zT
1

z @k C IA

T zN
2

iT

@SFSA @SFTA

xote tht in relityD not ll lndmrks my e visile from given root pose due to limittions in sensing rngeF hese eets n e inorported into the sensor unertinty model @see pigF SFP for n exmpleAF he mesurement mtrixD H D for the mesurement of ll lndmrks is
T T H @k C IA a H1 H2
h

T HN ;

iT

@SFUA

where the Hi @the individul mesurement toinsA re given y iqution @PFPWAF es desried in lter setionsD the primry onern is to minimize the root9s estimted pose ovrine t the golF his mkes it unneessry to trk expliitly the root9s position estimte for given pthF herefore the stte updte will not e used in nding the optiml pthD ut the ovrine updte @eqution @PFISA will e usedF sn order to void needless omputtion of the ulmn gin mtrix K D the following form of the ovrine updte is usedX
P @k C I=k C IA a P @k C I=kA P @k C I=kAH T S 1 HP @k C I=kA;

@SFVA

where the S mtrix is desried in iqution @PFIRAF

5.2 Cost Function

US

e primry onern is hving the root relily reh given gol positionF por exmpleD the root my e le to reognize it hs rehed the gol only when it is within ertin rngeD D of the golF his would imply tht minimizing the unertinty of the root9s position t the gol should e primry ojetiveF s hoose norm on the root pose ovrine t the gol s the ost funtion of the optimiztion prolemD iFeFD
J a jjPRR @M=M IAjj;

@SFWA

where M is the numer of steps the root hs tken to reh the gol from given strting positionF eny norm n e hosenD eh hving its own interprettionF s use the proenius normD whih for mtrix A is dened to eX
jjAjjF a
q

tre@AAT A
i

i2 ;

@SFIHA @SFIIA

where the i re singulr vlues of the mtrix AF he proenius norm yields good numeril ehvior euse it does not involve ny nonEsmooth mx or min opertionsF st lso hs n intuitive interprettionF sn this se the mtrix A a PRR is symmetri nd positive deniteD so its singulr vlues re the sme s its eigenvluesF gonsequentlyD this norm provides the lestEsqures minimiztion of the priniple vlues of the root9s positionl unertinty ellipseF ith the ost funtion hosenD the optimiztion prolem is dened elowF vet @tA e root pth from the strt to the golD iFeFD @HA a xR @HA nd @tmA a xg F vet p denote the losure of freespaceD the re of the environment not oupied y ostlesF hene the fesile pth setD D s the set of pths tht lie entirely in p X
a j @tA P p Vt P H; tm :

@SFIPA

UT he prolem is to nd the lowestEost fesile pthX min J @ @tM AA sujet to @tA a u:
(t)P

@SFIQA @SFIRA

he following setions desrie the method to solve this optimiztion prolemF

5.3 Practical Optimization Approach

goneptullyD given the ulmn lter frmework desried in etion SFID one ould prmetrize the root9s pth in terms of the veloity ontrol inputsF qiven sequene of ontrol inputsD the ovrine of the stte estimte n e found y the pplition of the ulmn ltering equtions given in etion PFPF he root9s ovrine n e propgted long the proE posed pth y use of iqution @PFIHAF herever the root stops to perform mesurement of the lndmrksD iqution @PFISA would e used to lulte the the root9s new position estimte error ovrineF sing the lndmrks will redue the root9s position estimte error ovrineF feuse the root hs nite sensing rngeD it must e ner enough to the lndmrks to e le to use them for loliztionF he root ould perform this lndmrk mesurement t every time stepD or t smller numer of positions long the pthF he ontrols n then e vried throughout their fesile spe to nd the lowestEost pthD using the ost funtion given y iqution @SFWAF es desried elowD s hoose slightly dierent nd more prtil prmetriztion tht yields the sme eetF feuse of the positionEdependent mesurement modelD it is more onvenient to prmetrize the optimiztion prolem in terms of root positionsD or wypointsD insted of the open loop ommnd veloities V F e ollotion pproh is hosen where the root pth is disretized into smll numer of wypointsF en M step root pth will hve M I wypointsD whih re denoted y h i U a U U UM ; @SFISA
1 2 1

where Ui is single wypoint on the root pthD speied s the grtesin lotion @xi; yiAF here re few resons why the pth is disretized into smll numer of wypoints s opposed to nding the ontinuous pthF he rst is omputtionl onernF feuse s use grdient desent methodD the time required to minimize n initil guess grows

UU lrger nd lrger s the numer of degrees of freedom is inresedF he seond reson is more prtil oneF he wypoints re the lotions where the root will stopD tke mesurement of lndmrksD nd perform n updte to reElolize itselfF e relisti root would likely use sensors suh s lsers or mers to nd the lndmrksF he imge or sn proessing required to pik out the lndmrks nd solve the orrespondene prolem is muh greter thn the proessing required to propgte step of odometryF essuming tht the root n ontinully proess imge or sn informtion t high rtes is unrelistiF st would e nturl to prmetrize the ost funtion in terms of the ontrol inputs V F roweverD suh prmetriztion would require prmetrizing the mesurement ovrine mtrix R in terms of the ontrol inputs s wellD or ssuming tht the mtrix is onstntF essuming tht the mtrix R is onstnt everywhere is n unresonle ssumptionF hile the mesurement noise my e reltively onstnt in region round prtiulr lndmrkD ny rel sensor is ound to hve limit to its rngeF yne wy to llevite this prolem is to mke the mesurement noise funtion of the lndmrk eing mesured nd the position of the rootD eFgFD Ri @k C IA a f @xR @k C IA; xLi @k C IAA : @SFITA his funtionl form llows for mny dierent ssumptions for the mesurement noiseF rtilly useful forms for Ri would inlude qudrti or exponentil dependene on the sensing rnge to LiF en exponentil dependene provides prtiulrly onvenient wy to inlude limittions in sensing rnge while keeping the ost funtion smoothF wny ommon sensorsD suh s lser snnerD hve nite sensing rngeF ithin the sensing rngeD the proess noise is reltively onstnt IF roweverD modeling the noise s rpidly rising exponentil outside of tht rnge eetively inorportes limited sensing rngeF wking n ssumption suh s @SFITA llows the ulmn lter updte step @iqution @PFISAA to e prmetrized in terms of the root9s pthF o keep onsistent set of vrilesD the propgtion step @iqution @PFIHAA is modied to use the sme optimiztion vrilesF wking the ssumption tht the vrine in the root9s veloity is proportionl to its veloityD iFeFD v a V; @SFIUA where  is proportionlity onstntD hnges the vriles of the propgtion step to e the pth wypointsF fy sustituting iqutions @PFPRA into iqution @PFIHAD the ovrine

UV propgtion eqution tkes somewht simple formX

Q@kA P @k C I=kA a P @k=kA C t2 R
P

H N
2

H N S; H N N
2 2 2 2

@SFIVA

where Q@kA is s in iqution @PFPPAF ith @SFIUA sustituted inD the root9s portion of the ovrine mtrix propgtion step is
 v @k A PRR @k C I=kA a PRR @k=kA C t2 R x
2 2

 vy @kA
2 2

Q S:

@SFIWA

sing the ft tht

vx @kAt

a vy @kAt a

yR @k C IA yR @kA;

xR @k C IA xR @kA;

iqution @SFIWA simplies to

PRR @k C I=kA a PRR @k=kA C 2 P @k C IA;

@SFPHA
Q

where the ovrine propgtion inrement P @k C IA is dened s H S: P @k C IA a R@xR @k C IA xR @kAA H @yR @k C IA yR @kAA
2 2

@SFPIA

he root is impliitly onstrined to seek the gol during the nl stepF reneD the nl propgtion inrement is H S: P @M A a R@xg xR @M IAA H @yg yR @M IAA
2 2

he rst propgtion inrement P @HA is similrD ut is funtion of the root9s strting lotionD x F he gol of ghpter T will e to show how to inorporte these ides into true sensorE sed motion plnning lgorithmF o do thisD the lgorithm must inorporte sensing of
0

UW ostlesF st is ssumed tht ostles lok sensing of other ostlesD ut not sensing of lndmrksF yne wy to hndle ostles in the optimiztion proess is to use them s onstrintsF ngent fug n e thought of in this wyD where the ost funtion is simply the pth lengthD nd the viewle ostles re stte onstrintsF enother possiility would e to ugment the ost vi penltyEfuntion SI for violting the onstrintsF s hoose the formerD iFeFD the pth nnot interset ny ostlesF por ll simultionsD the ostles re simply modeled s irlesF por illustrtion purposedD spei formuls for inorporting disk ostles s onstrints re inluded in eppendix fF he optimiztion prolem n now e dened in more detil s followsX qiven strt lotionD gol lotionD nd the numer of pth steps M D nd the set of wypoints U ; U ; : : : UM tht minimize the ost funtion J @U AD s dened in @SFWAF here re multiple methods one ould use to minimize the ost funtionF yne possiility is minimizE tion vi enumertion PRF sf the wypoints were disretized into grid with CU ellsD the ost of every pth of M steps ould e lultedF his would e gurnteed to yield the M minimum ost pthD ut it hs omputtionl ost of O@CU A PRF iven for PH PH gridD whih is quite smllD there re TR million pths with three wypointsF o keep omputtion times to minimumD grdient desent method is usedF yf ourseD using grdient desent will only nd lol minimum of the ostD not neessrily the glol minimumF e seprte step is required to identify ll of the lolly miniml pthsF
1 2 1 1

5.4 Initial Condition Generation

sing grdient desent method to solve the optimiztion prolem implies tht the solution will only e lol optimumD nd not neessrily the glolly optiml solutionF hih lolly optiml solution is onverged upon is highly dependent on the initil onditionsF o improve the hne of nding the glol optimlD the lgorithm rst genertes numer of initil onditions tht tke dierent4 pths through the environmentF ih of these initil onditions is optimized in turnD nd in the end the one with the lowest ost is hosenF s propose nd use simple heuristi to generte vriety of initil onditionsF pirstD the lol tngent grph @vqA t the root9s strt lotion is onstrutedF his onstrution ensures tht there re pths tht leve on oth sides of physil ostles in the root9s viinityF o generte one set of initil pthsD pth is onstruted tht goes from the root9s

VH strt position to n vq nodeD then to lndmrkD nd nlly to the golF hese pths n e thought of s ones tht detour to tke dvntge of the lndmrksF his is repeted for ll omintions of vq nodes nd lndmrksF sf there re n nodes in the vq nd m lndmrksD this rst set will ontin n m initil pthsF enother set of pths is reted tht goes diretly from the root9s urrent position to lndmrkD to seond lndmrkD then diretly to the golF his set of pths is similr to the rst setD ut it is not onstrined to pss ner n ostleF e third set of pths is reted tht goes from the root strtD to n vq nodeD then diretly to the golF hese pths n e thought of s tking the diret route round n ostle to the golF his seond set will ontin n initil pthsF sn mny situtionsD multiple initil onditions will onverge to the sme solution @within numeril uryAF here re mny other options tht one ould use to rete more initil onditionsF snsted of initil pths tht visit only two lndmrksD pths tht visit m lndmrks ould e genertedF feuse the root hs omplete knowledge of the ostlesD reting the vq with lrger sensing rdius thn the root9s sensor hs ould e used to rete initil pths tht weve4 round dierent sides of ostlesF e ompletely dierent onstrutD suh s the generlized oronoi grph IH ould e used lso to rete initil pths tht pss on diering sides of ostlesF yf ourseD the more initil onditions tht hve to e optimizedD the slower the overll lgorithm will eomeF
5.5 Results

o illustrte the performne of this methodD this setion presents sequene of simultion results tht involve dierent numers of wypointsD lndmrksD nd ostlesF hese simuE ltions model point root with n onEord lndmrk sensor whose unertinty model hs n exponentil dependene on the rnge to the lndmrkF peillyD the rngeEdependent mesurement vrine follows the formX
i2 @k C IA a  C exp @ @ri @k C IA rmax AA ;

@SFPPA

VI
10
3

10 Measurement Variance

10

10

10

0.5

1.5

Range

2.5

3.5

pigure SFPX mple mesurement vrine prole s funtion of rngeF  a :ID a IHD nd rmax a QF xote tht the yExis sle is logrithmiF where  nd re hosen to give relisti proleD rmax is the sensor9s mximum useful rngeD nd ri is the rnge to the lndmrk LiD iFeFD
ri2 @k C IA a xR xLi
h iT h

xR xLi :

@SFPQA

he prmeter  n e thought of s the nominl mesurement vrineD while ets how quikly the exponentil inresesF hen the mtrix Ri@k C IA is simply
P

Ri @k C IA a

2 @k C IA R i

H S: i @k C IA
2

@SFPRA

en exmple of this prole is shown in pigure SFPF st n e seen tht s the mesurement rnge inreses pst rmaxD the noise vrine inreses drmtillyF prom the perspetive of using the mesurement in ulmn lterD the lndmrk eomes invisileF his model llows the sensor9s nite rnge to e tken into ountD while mintining ontinuity of the vrineF

VP he following results onsider dierent senrios where the root goes out of its wy to use the lndmrksF por ll exmplesD the initil ovrine of the root is zeroD nd ll lndmrks hve initil ovrines of
:I PLiLi a R
P

HS: H :I

@SFPSA

edditionllyD the vlue of the se mesurement vrine for iqution @SFPPA  a :ID the sensor rnge rmax a QD nd the multiplier a IHF pinllyD the odometry noise proportionE lity onstnt  a H:ISF hese re ll resonle numers for rel moile rootF ell simultions were implemented in wtl using the fmincon funtion to perform the minimiztion of the ost funtionF he fmincon funtion nds the minimum of onstrined nonEliner multiEvrile funtionF he omputer used for ll simultions is PFVqrz entium R proessor with IFS qf of ewF
5.5.1 Robot Drives Past the Goal

he rst exmple onsists of only single lndmrk with no ostlesF he lndmrk is loted so tht it is out of the sensor9s eetive rnge for ll points long the strightEline pth to the golF he simultion result @pigure SFQA illustrtes the utility of using the lndmrk in the pth plnning proessF sf the root were to tke the stright line pth to the golD it would not get lose enough to the lndmrk to use it for loliztionF rowever the pth tht minimizes the ost tkes detour to use the lndmrkF he nl ost for the detouring pth is HFHUSF por omprison purposesD the nl ost for the stright line pth @in this se the shortest pthA is HFHWF hile this is not lrge derese in the ostD it illustrtes the vlue of using this method in even simple situtionsF por this simple exmpleD there were three initil onditions genertedF he time required to optimize ll initil onditions ws HFUQ seondsF st is interesting to note tht the pth only goes lose enough to the lndmrk to enter the region where the sensing noise is reltively onstntF here is no need to drive ll the wy to the lndmrkD s doing so would only inrese the ost due to the extr odometri errorF

VQ

pigure SFQX ixmple where the root initilly drives pst the gol in order to use lndmrkF por simpliityD the sensing rnge is plotted out the lndmrk E if the root moves inside of tht irleD it is lose enough to see the lndmrkF
5.5.2 Robot Follows the Landmarks

he seond exmple shown in pigure SFR onsists of senrio with multiple lndmrks nd no ostlesF here re four wypoints for this exmpleF here were QI initil onditions generted for this exmpleD nd totl run time of QWFS seondsF he ost for the optimized pth is HFHRWD nd the ost for the strightEline pth is HFHWF husD there is sustntil redution in nl unertinty when lndmrks re tken into ountF ixept for the two lndmrks nerest the golD the other lndmrks in this exmple re fr enough prt tht no two lndmrks n e seen t one y the rootF feuse of thisD the optiml pth tkes short steps from one lndmrk to the nextF enother interesting result is the lotion of the third wypointF st lies in n re where multiple lndmrks re within the sensing rngeF snorporting mesurements from multiple lndmrks during n updte llows for greter redution in the estimte error thn using just one lndmrkF st mkes sense tht these multiEview regions re optiml lotions for n updteF

VR

pigure SFRX e multiEstep pth where the root signintly devites from the shortest pth in order to use the lndmrksF
5.5.3 Examples with Obstacles

ystles re used s onstrints during the optimiztion proessF he results from epE pendix f were implemented to inorporte disk ostles into the simultionF pigure SFS shows n exmple with some ostles in the environmentF st is similr to pigure SFQ in tht there is lndmrk neryD ut pst the golF he ost for the optimized pth using the lndmrks for loliztion is HFHQWD while the ost of the shortest pth is HFHWIF his exmple hd QI initil onditionsD nd the totl run time ws PWWFT seondsF es in the rst exmpleD the root does not go diretly to the golD ut rst uses the lndmrk L ner the golF his timeD the optiml pth lso psses y the lndmrk L to further redue the expeted unertinty t the golF his exmple demonstrtes tht even in the presene of ostle onstrintsD the optimiztion proedure still nds lowEost pth through the environmentF pigure SFT shows results for the sme lndmrk ongurtion s tht of pigure SFRD ut with ostles in the environmentF he ost of the optimized pth using the lndmrks
5 1

VS

pigure SFSX en simple exmple with ostlesF is HFHSTD while the ost of the shortest pth is HFHWQF here were QI initil onditions for this exmpleD with totl runtime of IPI seondsF he ostles were pled suh tht they loked the originl optiml pthF es the gure showsD the optiml pth still follows the tril of lndmrksD ut it lso losely skirts the ostle oundriesF he optimiztion proedure uses the lndmrks for loliztionD while still ttempting to keep the pth s short s possileF
5.6 Summary and Discussion

his hpter presented n optimiztion method tht minimizes the root9s expeted pose unertinty t the golF his oEline method ssumes full knowledge of ll ostles in the environmentF imultion results lerly show the utility of expliitly using lndmrks s prt of the pth plnning proessF his llows the root to go out of its wy4 to use lndmrks to id in its loliztionF th length is trded o with loliztion pilityF yne of the weknesses of this method is its reline on good initil guessesF feuse

VT

pigure SFTX he sme lndmrk ongurtion s tht of pigure SFRF ystles were pled to lok the originl optiml pthF the method is sed on grdient desent pprohD the lgorithm my end up missing lowerEost pth euse none of the initil guesses lie within the sin of ttrtion for tht prtiulr lol minimumF he heuristi desried in etion SFR performs resonly well most of the timeD ut of ourse it is not perfetF en interesting possiility for the future would e to understnd etter the properties of the ost funtionF here re n innite numer of pths tht go from the root9s urrent position to the gol nd pss through prtiulr pointF he root is only interested in the est4 pth tht psses through prtiulr pointD thoughF he surfe dened y the ost of the est pth through prtiulr point my hve properties tht ould e exploited to more eiently solve the prolemF roweverD this is just the rst step towrds ridging the gp etween the elds of sensorE sed motion plnning nd vewF sn order to mke this method fully sensorEsed lgorithmD the full knowledge ssumption must e removedF he root will hve to use its onEord sensors to inrementlly uild up its knowledge of the worldD nd ppropritely

VU repln its pthF he following hpter presents one lgorithm to do thisD lled nertin fugF

VV

Chapter 6
Uncertain Bug
his hpter presents the nertin fug lgorithmF nertin fug is sensorEsed moE tion plnner tht inorportes knowledge of lndmrks nd tkes into ount the root9s loliztion ility nd unertinty long pthF nertin fug mkes hevy use of the optimiztion proedure presented in ghpter SF he min dierene etween the setup for nertin fug nd the setup for the optimiztion proedure presented in ghpter S is tht the root does not hve prior knowledge of the lotion of ny ostles in its environmentF he loliztion unertinty tht the ost funtion ptures omes out from numerous souresF yne soure is the root9s imperfet odometryF he root my e integrting elertions nd ngulr rtes from gyro to estimte its positionF yr it my e ounting the numer of tiks on n enoderF egrdless of how the root performs its odometryD it will e n imperfet estimte whose unertinty grows with distne trveledF enother soure of loliztion error is the lndmrksF hen the root uses the lndmrks to lolize itselfD the lndmrk9s positions will e known impreiselyF iven if the lndmrks were known perfetlyD ny relisti sensing proess will return n imperfet mesurement of the lndmrk positionsF he si prolem setup is disussed in etion TFIF e desription of the nertin fug lgorithm is given in etion TFQD with prtiulr ttention given to the eets of unertinty on the motion plnning proessF roperties of the lgorithm re given in etion TFRD nd simultion results re presented in etion TFSF

6.1 Setup and Denitions

VW

fefore explining the nertin fug lgorithm the si setup of the prolem is desriedD whih prllels tht of the optimiztion method presented in ghpter SF ertinent detils nd importnt dierenes re presented in this setionD ut other similr spets re left out for revityF lese see ghpter S for the omplete setupF he position of the root with respet to the glol referene frme will e denoted y xR F he root is modeled s point moving in plneD so
xr xR a R S : yr
P Q

@TFIA

he root is instruted to move from its urrent position to some gol ongurtionD whih is speied in the glol oordinte systemF he gol ongurtion will generlly e speied s n @x; yA pir in the plneD iFeFD P Q xg xg a R S : @TFPA
yg

fefore strting its tskD the root is given the position of N lndmrksF he grtesin position of the ith lndmrk in the glol referene frme is denoted y xLiX
xLi xLi a R S : yLi
P Q

@TFQA

he lotion of the lndmrks my e unertinD nd the root is equipped with sensor ple of mking noisy mesurements of the lndmrk positions nd deteting ostlesF his sensor hs nite sensing rngeD RD unless otherwise notedF por prtiulrs on the lndmrk mesurementsD see etions SFI nd PFPFPF he stte of the entire system is simply the stte of the root nd ll lndmrksX
x a xR xL1
h

xLN :

iT

@TFRA

he estimte of the stte is not ssumed to e perfetF st is ssumed tht the root is equipped with n estimtor suh s ulmn lter @ee etion PFPA tht n provide estimtes of the stte s well s the stte estimte error ovrineF he estimte error

WH
L1

L2 xR r xg L3

L4

pigure TFIX etup ssumed for the nertin fug lgorithmF he root does not know the position of ostles in the environmentF he root9s sensor hs nite sensing rngeD rF ovrine will hve lok strutureX
P

a RPRR
PLR

PRL S ; PLL

@TFSA

where PRR is the P P ovrine mtrix of the root9s position errorD PLL is the PN PN T mtrix of lndmrk position error ovrinesD nd PRL a PLR re the rossEoupling error ovrinesF st is not ssumed tht the lndmrks re known perfetlyD so PLL need not e ll zerosF he root is ssumed to hve initil ovrines for ll lndmrk lotionsF hile the ost funtion used for ngent fug nd yptimEfug is the root9s totl pth lengthD nertin fug uses ost funtion tht ims to minimize the expeted positionl unertinty t the golX J a jjPRR @M=M IAjj; @TFTA where M is the numer of steps the root hs tken to reh the golD nd PRR @M=M IA is the ovrine of the root position estimte error t step M D given ll mesurements up to step M IF elso rell tht the norm is the proenius norm @ee iqution SFIIAF st is ssumed tht if the root is loser thn distne of  to the golD it will e le

WI to identify the gol nd the lgorithm will terminte with suessF resumlyD when the root is within  of the golD it ould use lol feedk lgorithm suh s visul servoing to guide its nl djustmentsF his ssumption ples limit on the @expetedA mximum mgnitude of the root9s position estimte error ovrine t the golF sn order to deide whether or not given pth meets this riteriD some level set of the root9s ovrine mtrix is hosenD nd the ssumption mde tht the root will lie in tht setF por exmpleD one ould pik the Q levelD whih trnsltes to WWFU7 hne tht the root will e in tht regionF vet Ug denote the mximum dimension of the root9s ovrine ellipse for prtiulr level setD iFeFD
Ug a c mx i @PRR @M=M IAA;
i
p

@TFUA

where c is prmeter tht desries whih prtiulr level set is hosenD nd i @PRR A is the i eigenvlue of the mtrix PRR F s generlly hoose c a QD lthough ny other level ould e hosenF sf prtiulr pth rehes the gol with Ug < D the pth is sid to e fesileD or hs acceptable uncertaintyF
th

6.2 Motivation and Background

elgorithms suh s ngent fug nd yptimEfug re useful euse of their ompleteness propertyF fut their ssumption of perfet dedErekoning mkes them diult to implement in relisti situtions nd renders ny proof of onvergene unworkle in prtieF hrun9s ostl nvigtion pproh is one step towrds omining the pth plnning prolem with loliztion methods RTD RUF hey model the informtion ontent of ll ongurtions in the environmentD inluding the possiility tht mesurements ould e orrupted @y previously unknown moving ostleD not just the sttistil proess noiseAF roweverD the ostl nvigtion pproh requires prior mp of the environment in order to rete this informtion mpF4 yne would like to e le to show tht if there is pth from the strt lotion to the golD then the root will nd this pth nd reh the gol even in the presene of unertin root positions nd noisy sensor mesurementsF nfortuntely it is not neessrily the se tht the root n reh the gol @even if the gol is rehleA with unertin position

WP mesurementsF he root must lern out the lotion of ostles in the environmentF hen enountering new informtionD the root must often mke hoies etween multiple lterntive pthsF st my mke hoie tht uses it to fil to e le reh the gol with some given level of positionl unertintyF por exmpleD if the root hd hosen to go left4 round n ostle insted of rightD4 it would hve rehed the gol suessfullyF his issue is disussed in more detil in etion TFQFIF
6.3 The Uncertain Bug Algorithm

he gol of the nertin fug lgorithm is to nd pth through n priori unknown environment so tht the root rehes the gol with eptle unertintyD even in the presene of sensing nd loliztion errorsF his setion presents short overview of the nertin fug lgorithmD followed y more detiled disussionF nertin fug hs sinE gle mode of opertion|tht of omputing the optiml pth to the gol nd following itF nertin fug uses the pth optimiztion proedure from ghpter SD ut does so in n inrementl mnnerF feuse the root does not know priori the lotion of ny ostles in the environmentD it must reElulte its pth when it enounters new ostle nd gins more informtion out the worldF sn summryD the min step of the nertin fug lgorithm opertes in the following mnnerX IF ke view of the worldD iFeFD ompute the urrent visiility set v@xk A nd then updte the totl visiility set Vtot with the newly gthered informtionF PF glulte the optiml pth to the gol @using the optimiztion proedure presented in ghpter SAD tking into ount knowledge of the environment otined so frF rtillyD this optimiztion step proeeds through these stepsX @A qenerte numer of initil guessesF @A winimize the ost of eh initil guessF @A ghek the ost of ll of the optimized initil guessesF sf none of them rehes the gol with Ug < D the gol nnot e rehed from the urrent position with eptle unertintyF erminte with filureF edditionl termintion riteri re disussed elowF

WQ QF pollow the optiml pth to the edge of the totl visiility setD or to the gol if the pth is ontined within the urrent visiility setF sf the root is within distne of  the golD terminte with suessF sf notD ontinueF nertin fug is similr to yptimEfug in tht it lwys omputes the optiml pth to the gol t eh step given urrently ville informtionF fut in nertin fugD the im is to minimize the root9s expeted position estimte error ovrine t the golF nertin fug relies hevily on the optimiztion pproh of ghpter SF ell tht this optimiztion proess required knowledge of ll ostles in the environmentF he ostles re used s onstrints|the pth nnot interset ny ostleF sn nertin fugD the root only tkes into ount the ostles tht it hs seen from its strting position to its urrent positionF es in the yptimEfug lgorithmD the root must rememer the lotions of the ostles it hs enountered in the environmentF his is simply inry ostle mpD where position is either impssle @n ostleA or pssle @freespeAF his memory requirement is due to the nture of the optimiztion proess|the ostles re used s onstrints in the pth optimiztion proessF he ostle informtion stored in the mp will lso e sujet to unertintiesF sssues tht rise from mp errors re disussed in etions TFR nd TFTF st is ssumed tht when the root is moving within VtotD it uses lol ostle voidne sheme to miss ny ollisions tht ould rise due to the ostle lotion nd nvigtion unertintyF es the root sees more nd more of the worldD it n more urtely lulte the optiml pth to the golD or determine tht the gol is unrehleF imilr to ngent fug nd yptimEfugD the root will never tke step eyond the region of the world tht it urrently knows outF st will only follow the urrent optiml pth to the extent of its urrent knowledgeF efter tht stepD the root will lern more informtion out the world tht will llow it to mke more urte lultion of the optiml pth to the golF sn some sesD the optiml pth will not hnge t llF sn other ses the root will see n ostle tht loks the optiml pth it lulted in the lst time stepF his new informtion will e inorported into the root9s mpD whih is then used to nd new fesile pth to the golD if one existsF

WR
6.3.1 Eects of Uncertainty

ell tht the min ide is for the root to reh the gol relilyF his is the motivtion for the hoie of the ost funtionF yne wy to visulize the eets of loliztion unertinty on the motion plnning proess is through level sets of Ug F gonsider surfe in R where the height of the surfe is equl to the vlue of Ug for the optiml pth from tht point to the golF ell tht  is the eptle unertinty t the golF ghoosing prtiulr vlue of  n e thought of s sliing through the unertinty surfe with plne t height of F por ny point where Ug !  is ongurtion from whih the root ould not reh the gol with eptle unertintyF sf the root knew the entire world geometry hed of timeD it ould lulte the ext shpe of this surfe nd immeditely report whether or not it ould reh the gol with eptle unertinty from its strt lotionF fut euse it does not know the ostle ongurtionD it n only mke guess t wht the surfe looks likeD given its urrent knowledgeF es it moves through the environment nd lerns more informtion out the ostle ongurtionD the shpe of the surfe tht is sed on ville knowledge will hnge s some pths eome inessile euse they re loked y physil ostlesF es previously notedD ny point on this surfe where Ug !  is point where the root nnot relily reh the gol within the given distne thresholdF gonneted regions where Ug !  n e thought of s uncertainty obstaclesD euse they re regions of the ongurtion spe tht the root must voidF sf the root ever steps into n unertinty ostleD it n no longer gurntee tht it will reh the gol with eptle unertintyD nd must report filureF st my seem tht the lgorithm should just mke sure tht it never ventures into one of these unertinty ostlesF nfortuntelyD the geometry of the unertinty ostles is funtion of the geometry of physil ostles tht my lie fr hed of the root nd outside of its urrent knowledgeF he diulty is tht euse the root doesn9t know the unertinty ostle ongurtionD it my not know it hs stepped into one until it is too lteF gonsider senrio where there is one ostle in the environmentD ut outside of the root9s initil sensing rngeF elso ssume tht the ongurtion of the lndmrks is suh tht there re only two lolly optiml pths tht reh the gol with Ug < D ut one of these pths psses through the ostleF his setup is shown in pigure TFPF feuse the
3

WS
xR xStart 1 L1

xg L2 2

pigure TFPX rypothetil exmple where the root hooses pth tht uses it to step into n unertinty ostleF root does not know out the ostle from its strt positionD it piks the pth with the smllest ostF essume tht it hooses the pth tht intersets the ostleD whih is in the gureF e few steps lterD the ostle enters the root9s sensing rngeF et tht pointD the root relizes tht the pth it ws following no longer rehes the gol with eptle unertinty euse of the dded pth length needed to irumnvigte the ostleF sn dditionD it hs rued enough unertinty tht it n no longer reEroute to the other pth nd still reh the gol with eptle unertintyF o the root will not know tht it is stepping into n unertinty ostle until it hs lredy done soF husD it is not generlly possile to gurntee ompleteness of the lgorithmF he strongest sttement tht n e mde out onvergene is tht the lgorithm will hlt in nite mount of timeF en interesting property of these unertinty ostles is tht they will only grow in size s the root lerns more out the environmentF hey will never eome smllerF vet p denote the region of freespe from whih the root n reh the gol with Ug < D iFeD
1

p a fq j q P @tA; @tA P g ;
2

@TFVA

where  is the set of ll pths tht reh the gol with eptle unertintyD @tA is prtiulr pthD nd q P R is root ongurtionF vet y denote the set of unertinty

WT ostlesF he unertinty ostles re the prts of freespe not in pD iFeFD

y a p n p:

@TFWA

ith no ostles in the worldD p ould e lulted extly from the strtF st would e funtion of lndmrk lotionsD sensing unertintyD nd odometri unertintyF fut s the root lerns out the lotion of more ostlesD the set of ville ongurtions q in p will e reduedF qoing k to the surfe nlogyD it is s if more nd more ongurtions end up ove the level set of Ug a F
6.4 Uncertain Bug Algorithm Properties

he following proofs of the properties of the nertin fug lgorithm will ssume tht the gol is not pled inside n ostleD iFeFD the gol is physilly rehleF hisussion of wht hppens when the gol is inside n ostle follows in etion TFTF o e le to sy nything onrete out the lgorithmD one must mke ertin sE sumptions out the performne of the underlying optimiztion proessF he optimiztion proess must return fesile pth if one existsF rtilly spekingD this mens tht the optimiztion proess is implemented perfetlyF yf ourse in relity this my e diultF feuse the optimiztion proess uses grdient desent methodD if there is not n initil ondition generted tht lies in the sin of ttrtion of the glol minimumD tht pth will e overlookedF qiven Vtot@xk AD the optimiztion lgorithm of ghpter S will return lolly optimlD fesile pth if one existsD or return with filureF
Axiom 6.1.

uess or filure of nertin fug depends on the size of the root9s ovrine t the golF sf the root9s method for trking this ovrine is fultyD then the root my think tht pth is fesile when in ft it is infesileF o it must e ssumed tht this ovrine reets the true estimte error ovrineF his mens tht ll of the prmeters tht et the estimte suh s the odometry noiseD the sensing noiseD nd the lndmrk position estimte ovrines re orretF
Axiom 6.2.

he root9s ovrine estimte is orretF

WU sf the ovrine estimte is onservtiveD iFeFD it lwys ounds the true estimteD then this ould use the root to premturely terminte with filure even if there still exists pth to the gol with Ug < F roweverD if the ovrine estimte is overondentD then the root my think tht the gol is still rehle with eptle unertinty when it is notF yne n ssume tht freespe is oundedD s ould e the se in n oeElike enviE ronment or the surfe of wrs @whih is lrgeD ut still oundedAF yn the other hndD few intuitive rguments n e used to ple tighter ound on pF
Lemma 6.1. Let there be N landmarks, each at a location @Lix ; Liy A, in a bounded set.
with a position of @xR ; yR A, and is equipped with a sensor with a nite range, R. The robot's odometry is assumed to be such that the variance in its velocity is proportional to the velocity, i.e., v The landmark positions are imperfect. The robot is modeled as a point operating in

R2

a V . The goal is at a position of @gx; gy A.

Under these conditions, the region of freespace from which the robot can reach the goal with Ug <  is bounded.

qiven the setup desried ove nd further desried in etions SFI nd TFID p is ounded y the union of the squres given y the following inequlitiesX
Proof.

mx @jLix xR j; jLiy yR jA mx @jgx xR j; jgy yR jA

< <

Q Q

@TFIHA @TFIIA

ht isD if ny of the inequlities re violtedD then the root is gurnteed to not e in pF herivtions of these onditions re given in eppendix hF
Lemma 6.2. At each step, the robot will see a new part of the world, i.e., Vtot will grow,
or the algorithm will terminate with failure due to localization errors.

he proof follows tht of vemm RFI for yptimEfugD with n dditionl termintion ondition used y loliztion errorsF he root is ssumed to hve moved to xk from xk F fy denition of the lgorithmD the root will lwys follow the urrent optiml pth to the edge of VtotF elthough the root is ssumed to move distne of  @not neessrily the sme  within whih the root must reh the golAD euse the sensing rngeD RD is
Proof.
+1

WV greter thn D there will e niteEsized region of p tht the root will view for the rst timeF es long s the termintion riteri disussed elow is not violtedD the root is gurnE teed to see new prt of the environmentD nd s suh Vtot will grow t eh stepF sf the dditionl unertinty rued y moving from xk to xk is greter thn R D then it nnot e gurnteed tht the root will see new prt of it9s environment t step k C IF hen this oursD the lgorithm must terminte with filureF ytherwiseD y nlysis of vemm RFID the root will see new portion of the environmentF
+1

his dditionl termintion ondition n e illustrted s followsX gonsider n exmple where the root hs mpped reltively lrge VtotF ell tht the lgorithm dittes tht the root will lwys follow the optiml pth to the edge of VtotF sf t the next step the root drives long distne ross VtotD the loliztion errors rued from tht step lone ould eome lrge enough suh tht the lgorithm nnot gurntee tht the root will see new prt of p F
O1
xR Vtot L1 xg

x2

O2

L2

pigure TFQX pigure illustrting n extr termintion ondition for nertin fugF he next root pth is shown y the dshed lineF he root will follow the pth to the point leled xF
2

e hypothetil senrio where this ondition ould rise is illustrted in pigure TFQF he next root pth is shown y the dshed lineF fy denition of the lgorithmD the root will step to lotion x F sf ny dimension of the ovrine ellipse grows y vlue of more thn R D then tht mens it would e possile for the root9s true position to lie t the enter
2

WW of one of the pst visiility setsD vk D in whih se the root will not see new prt of the environmentF
Proposition 6.3. Uncertain Bug will not terminate with failure unless no feasible path to the goal exists from the robot's current position, xk , or localization errors are too large to
continue.

fy exiom TFID the optimiztion lgorithm will return fesile pth to the gol if one existsD or return with filureF his gurntees tht the root will lwys get new pth if the gol is still rehle with Ug < F elterntivelyD the lgorithm will return with filure if pth does not existF hereforeD nertin fug will not terminte with filure unless no fesile pth existsD or the loliztion errors re too lrge to ontinueF
Proof.

Proposition 6.4. Uncertain Bug will terminate in a nite amount of time.

sf the root knew the ongurtion of ll ostles prior to strtingD it ould immeE ditely lulte whether or not there exist ny fesile pths to the golF fut nertin fug does not ssume prior knowledge of ostles|the root must mp the ostles s it ttempts to reh the golF feuse of vemm TFID the totl re tht the root could mp out is niteF feuse of vemm TFPD the root will see new portion of this ounded re t every stepF sn the worst seD the root will explore ll of p efore determining tht the gol is rehle or notF his will our in nite mount of timeD euse the root sees newD niteEsized portion of p t every stepF he root will reh the gol efore mpping ll of pD or terminte fter nite numer of steps euse ll of p hs een seen nd no pth to the gol exists with eptle unertintyF herefore nertin fug will terminte fter nite mount of timeF
Proof.

wo dierent termintion senrios n our s result of errors introdued into the mpF hese errors re diretly used y the root9s positionl errorF roweverD the mp errors only et ses where the optiml pth is very ner the threshold of Ug > F sf the optiml pth hs Ug ( D smll errors in the mp will not et it gretly enoughF sn one seD the root my think the gol is not rehle with eptle unertinty euse n ostle pled @inorretlyA in the mp loks ny pth with Ug < F sf the

IHH ostle were pled orretly in the mpD the root would determine tht the gol is still rehle with eptle unertintyF sn this seD nertin fug will still terminte in nite mount of timeF elthough the gol is rehle with Ug < D the root hs no wy of knowing thisF he mp it uilds is the est informtion it hs villeF e seond sitution n our where the root thinks the gol is still rehle with eptle unertinty when it is notF sn this seD the errors in the mp led the root to e overondentF he root ontinues following the optiml pth to the gol @whih hs Ug > AD ut will eventully disover new informtion tht leds it to onlude the gol is not rehle with eptle unertintyF por exmpleD the root9s lol ostle voidne sheme ould use it to drive frther thn expetedF husD the root will eventully determine tht the gol is not rehle with eptle unertintyF elthough errors in the mp n use undesirle termintionD nertin fug will terminte in nite mount of timeF
6.5 Simulation Results

imultions hve een performed using the optimiztion lgorithm s presented in etion SF ell tht this method uses grdient desent to optimize numer of initil onditionsF he initil guesses re reted using the heuristi desried in etion SFRF epresenttive results re presented to illustrte performne of the lgorithmF por ll simultionsD the vlue of D the odometry noise prmeterD ws set t H:ISF ell tht n exponentil model for the sensing noise is used in order to help keep the ost funtion smoothF ee etions SFQ nd SFS for detilsF por these sensing noise prmetersD  a H:ID nd a IHF ell initil lndmrk ovrines were set to diag@H:I; H:IAF he root9s sensing rnge is Q unitsF xote tht in ll plotsD lndmrks hve irle of rdius equl to the root9s sensing rnge plotted round themF o if the root lies within the irleD then it n see the lndmrkF ell tht it is ssumed tht ostles lok sensing of other ostlesD ut tht ostles do not lok sensing of lndmrksF pigure TFR shows omplete simultion run of nertin fug with two ostles nd three lndmrks in the environmentF he root does not know out the ostles when it eginsF por this exmpleD the root requires IU steps to reh the golF he nl ost t the gol is only QW7 of the ost of the ngent fug pthD iFeFD the root rehes the gol with

IHI

pigure TFRX e simultion of nertin fugF he ngent fug pth is shown s the pink dshed lineF pinl ost t the gol is QW7 of the ost of the ngent fug pthF muh less positionl unertinty thn ngent fugF feuse the pth is hosen to exploit the lndmrks for etter loliztionD the root hs muh redued position estimte error t the golF pigure TFS shows detiled results from the ninth nd tenth steps long the root9s pth to the golF hen the root plns step numer WD it does not know out the ostleF feuse of thisD the optiml pth from the root9s strt position for step W psses through the ostleF pon exeuting step WD the ostle is within the root9s sensing rngeF his ostle is inorported into the root9s mpD nd the new pln tkes this ostle into ountF tep IH ppropritely voids the ostleD while still ttempting to use the lndmrks for n improved position estimte t the golF pigure TFT shows results from simultion run of nertin fug with nine ostles in the environmentF he nl ost of the nertin fug pth is only QP7 of the ost of the ngent fug pthF es in previous exmples from ghpter SD the root mkes use of the lndmrk ner the gol @L AF st my seem tht the root should follow the lolly shortest pth @the ngent fug pthA to lndmrk L D s opposed to the pth it hoosesF sn this
5 1

IHP

pigure TFSX he plnned pth for steps W nd IHF xote tht when the root sees the ostleD it reEplns the pthD tking into ount the new informtion out the worldF exmpleD there ws no initil ondition generted tht pssed etween the ostles like the lolly shortest pth doesF
6.6 Discussion

his hpter presented new sensorEsed motion plnning lgorithm lled nertin fugF nertin fug ims to minimize the expeted root position estimte error ovrine t the golD in order to improve the hnes tht the root will e le to move lose enough to the gol to reognize itF qiven the ssumptions presentedD nertin fug is gurnteed to terminte in nite mount of timeF nfortuntelyD it is not possile to gurntee tht

IHQ

pigure TFTX e simultion of nertin fugF he ngent fug pth is shown s the pink dshed lineF pinl ost t the gol is QP7 of the ost of the ngent fug pthF the root will reh the gol using nertin fugD even if pth to the gol existsF sf the gol is not physilly rehleD iFeFD it lies inside of n ostleD nertin fug will still terminte in nite mount of timeF he ft tht tht mp itself is unertin mkes this sitution muh more diult to reognize thn in the se of perfet dedErekoning @suh s ngent fug or yptimEfugAF o gurntee tht the gol is enlosed in n ostleD the root would need to drive n extr distne round the ostleF his extr distne would e on the order of the root9s positionl unertintyF sn eetD the root would e mking sure tht it hs seen enough overlp4 in the ostle oundry to e ertin tht the ostle is losed round the golF sf the root hd the pility to reognize fetures on ostlesD then it ould verify tht it hd irumnvigted n ostle y identifying the sme fetureF his type of lgorithm tht tkes the root9s loliztion pility into ount s prt of the plnning proess would e useful in wideErnging situtionsF por rooti plnetry explortionD rover my hve smll numer of lndmrks from either n overhed stellite or from rover tht hs nvigted the sme re in the pstF enother senrio is tht of underwter nvigtionF nderwter environments typilly hve sprse numer of fetures

IHR tht n e used for loliztionF foth of these exmples ould enit from n lgorithm suh s thisF hile s hve outlined the dierent termintion onditionsD the root ould still ontinue seeking the gol one some of the termintion riteri re metF he termintion onditions tht del with the size of the root9s unertinty do not gurntee tht there is not pth tht rehes the golD just tht there is no pth tht rehes the gol with eptle unertintyF st is entirely possile tht the root ould ontinue plnning pths to the golD get lukyD4 nd nd the golF yf ourseD the root ould lso ontinue for n indeterminte mount of time while trying to do thisF purther insights ould e gined into the properties of the lgorithm if one onsidered the limit s unertinty goes to zeroF yf ourseD if there is no unertintyD the ost funE tion will e zero everywhereF he urrent ost funtion impliitly enodes pth lengthD s longer pths will rue more odometri unertintyF his nlysis ould require dding n independent pthElength prmeter to the ost funtion to ount for thisF enother possile extension of this work would e to determine if there re ses where ompleteness ould e gurnteedF ss there some single @or smll numerA prmeter tht desries the dierent vlues of the noise @odometryD sensingD lndmrkA tht n e used to prove ompletenessc por exmpleD if one n ple worstEse ound on the mximum level of positionl unertinty in the environmentD wht other onditions need to e met to e le to gurntee tht the root will reh the gol if the gol is in ft rehlec

IHS

Chapter 7
Conclusion
7.1 Summary of Contributions

s hve developed nd presented new tools nd lgorithms for moile root nvigtionF he weighted snEmthing method of ghpter Q n e used s muh improved form of odometryF yptimEfugD presented in ghpter RD is omplete nd orret sensorEsed motion plnning lgorithmF ghpters S nd T desrie method to nd the pth tht minimizes the root9s position error t the golD nd n ssoited sensorEsed lgorithmD nertin fugD tht uses this optimiztion methodF he weighted snEmthing lgorithm is new method for estimting root displeE ment sed on dense rnge mesurementsF sn prtiulrD s investigted the eets of difE ferent error nd noise soures on the onvergene nd ury properties of these motion from struture lgorithmsF ixperiments showed tht reful ttention to the detils of error modeling n signintly enhne overll displement nd ovrine estimtion uryF yptimEfug is omplete nd orret sensorEsed plnnerF he lgorithm is sed on nding nd following the shortest pth in the environmentF yptimEfug ssumes point root with sensor tht hs nite rngeD nd requires tht the root hve perfet dedE rekoningF st lso needs the root to keep memory of ostles it hs seenF yptimEfug is gurnteed to reh the gol in nite mount of time if it is rehleD or terminte in nite mount of time if the gol is not rehleF wy pth optimiztion method minimizes the root9s expeted pose unertinty t the golF his oEline method ssumes full knowledge of ll ostles in the environmentF imultion results show the utility of expliitly using lndmrks s prt of the pth plnning proessF his llows the root to go out of its wy4 to use lndmrks to id in it9s

IHT loliztionF th length is trded o with loliztion pilityF nertin fug is new sensorEsed motion plnning lgorithm tht tkes the root9s loliztion pility into ount when plnning the pthF he lgorithm ims to miniE mize the expeted root position estimte error ovrine t the golD in order to improve the hnes tht the root will e le to move lose enough to the gol to reognize itF nertin fug is gurnteed to terminte in nite mount of timeF nfortuntelyD it is not possile to gurntee tht the root will reh the gol using nertin fugD even if pth to the gol existsF
7.2 Future Directions

his thesis suggests ontinued reserh into mny res relted to moile root nvigtionF es presentedD yptimEfug ssumes the root is operting in plnr environmentF yptimE fug9s pproh of lwys following the shortest pth given urrent informtion my extend well to higher dimensionsF sn higher dimensionsD the golEtngent lines would eome surfes tht re tngent to ostles nd the golF snsted of uilding up n ostle one segment t timeD the root would mp out the ostle one pth4 t timeF he pth optimiztion pproh of ghpter S is hevily dependent on the initil onE ditionsF fetter understnding of the ost funtionD s well s the pth spe4 tht the optiml solution lies in ould provide insight into hoosing etter initil onditionsD or using something dierent from grdientEdesentF yne possile pproh to this would e to nd if there is n unertin nlog to the golE tngent linesF sn yptimEfugD it is known tht the optiml pths follow golEtngent linesF sn the unertin seD the optiml pths re generlly not tngent to ostle oundriesF sf there were onstrut similr to the golEtngentsD they ould e exploited to improve the lgorithm performneD nd perhps even prove ompleteness in some situtionsF st is unfortunte tht nertin fug is not gurnteed to reh the golF erhps there is single prmeter or smll fmily of prmeters thtD under ertin situtionsD n e used to gurntee tht the root will reh the golD even in the presene of odometriD sensingD nd lndmrk unertintiesF por exmpleD if one ples ound on the mximum root positionl unertinty in the environmentD would tht llow ompleteness to e provenc

IHU

Appendix A
Weighted Scan Matching Derivations
A.1 Weighted Translation Solution

ell the logElikelihood formul of iqF @QFISAF ine Dij is independent of xij nd yij D the neessry ondition for n extreml in the logElikelihood funtion with respet to the vrile pij a xij yij T is
nij

rpij @M ij A a H D rpij @"ij AT @Pkij A "ij a H D k k

1

k=1 nij h k=1

rpij @"ij AT @Pkij A "ij a H D k k

1

P
nij h k=1
1

nij h k=1

ij I @Pk A1 "ij k

aH D

@Pkij A @~ ik Rij ~ jk pij A a H: u u

errnging this formul results in iqF @QFITAF

A.2 Weighted Rotation Solution

qiven n initil estimte of the trnsltionl displement pij D the rottionl displement n e derived y mximizing the likelihood funtion in iqF @QFIPAD or equivlentlyD the

IHV logElikelihood funtion in iqF @QFISA with respet to ij a D iFeFD

@M ij @A @

a H:

@eFIA

snsted of diretly omputing the grdient of M ij with respet to D we lulte it s followsX @M ij @A @M ij @ C A @ @A @M ij @A a @ @A a @ @A ; @eFPA @ @ where we used the reltion
 a  C  A

rere we derive n ext expression for the quntity M ij s funtion of F prom the ylor series expnsion for the funtions sin nd os we hve I I os  a os  I3 sin   P3 os   C ; I I sin  a sin  C I3 os   P3 sin   :
2 2

@ @ @A

a I:

@eFQA

ustituting in iqF @QFPAD the rottionl mtrix Rij n e written s I J I I I J C ::: Rij @A; Rij @A a I C I3 P3 Q3

2 3

where J is dened in iqF @QFIWAF he error "ij etween two orresponding lser pointsD k dened in iqF @QFSAD n e desried s funtion of the orienttion error X
"ij k

a a

uk uk ~ i pij Rij ~ j J Rij ~ j  uk I u C P3 Rij ~ jk  C :

2

~ i pij Rij ~ j uk uk

I I3

@eFRA

IHW he ovrine mtrix for the mthing error t the kth point orrespondene of poses i nd j in iqF @QFIHA n lso e desried s funtion of X
ij Pk @A

eij eij eij C @J Sk Sk J A C Sk eij eij eij eij @Sk C J Sk J A P @J Sk Sk J A Q eij eij C I @Sk C J Sk J A C ; Q

Qij k

@eFSA

where ij T eij Sk a Rij @ASk Rij @A: he inverse Ikij @A a @Pkij @AA of the ovrine mtrix n e omputed using ylor series expnsion s
1

ij ij ij ij Ik @A a Ik (0) @HA C Ik (1) @HA C Ik (2) @HA2 C :::

I P3

@eFTA

with
ij Ik (n) @HA

@ n @I ij @AA @ @An

     

;
=0

where
ij Ik (1) ij Ik (2) ij Ik (0) @HA
eij a @Pkij @HAA a @Pkij A a @Qij C Sk A ; k eij eij eij eij @HA a @Qij C Sk A @J Sk Sk J A@Qij C Sk A ; k k eij eij @HA a PIkij @HAPkij @HAIkij @HA C P@Sk C J Sk J A:
1 1 1 1 1 (1) (1)

IIH fy sustituting from iqFs @eFRAD @eFTA to iqF @QFIQA we hveX

M ij
nij a I fpT Ikij @HApk Pk k h i C PpT Ikij @HAJqk C pT Ikij @HApk  k k h T C pT Ikij @HAqk qk JIkij @HAJqk k ! T I ij @HAJq C I pT I ij @HAp  Ppk k k k Pkk C ::: g;
=1 (1) (1) (2)

@eFUA

where
pk qk

kpk k << kqk k:

u a ~ ik pij Rij ~ jk ; u u a Rij ~ jk ;

@eFVA @eFWA @eFIHA

xote tht there hs een no pproximtion mde up to this pointF iqF @eFUA is omplete expression of the ost funtion Mij D expressed s n innite series of terms polynomil in the orienttion estimtion error F sn order to minimize this funtionD we pproximte it fter onsidering limited numer of termsF por smll errors in the initil orienttion estimte @ < =TAD seondEorder pproximtion is suient when lrge numer of point orrespondenes re villeF righerEorder pproximtions re neessry s the numer of point orrespondenes deresesF fy sustituting iqF @eFUA in iqF @eFPA nd employing iqF @eFIHA we derive the expression for the orienttion displement error of iqF @QFIVAF
1

A.3 Covariance Estimation

T rere we onsider the estimtion prolem where nij mesurements Z a Z T ::: Znij T @with Zk a @~ i AT @~ j AT T A re proessed to derive n estimte of vetor  of the motion uk uk
1

1 Eq. (A.10) expresses the fact that the point correspondence errors are very small compared to the distances to these points.

prmeters aR
P

III
pij ij
Q S P

aR

hp @Z A h @Z A

Q S

a h@Z A

@eFIIA

with the expressions for funtions hp nd h given y iqutions @QFITA nd @QFIVAF e rstEorder pproximtion of the error in the estimte of the prmeter vetor  is given y
" a b

rT h Z

@ Z A "Z a

nij k=1

rT k h@Zk A "Zk Z

@eFIPA

with
rT h@Z A a rT 1 h@Z A ::: rT nij h@Z A Z Z Z
h i

@eFIQA

nd
rT h @Z A Z rT k h@Z A a R T k p S : Z rZk h@Z A
P Q

@eFIRA

xote tht
E f" g a E frT h@Z A "Z g a rT h@Z A E f"Z g a ~ 31 : H ^ Z Z
he ovrine of the estimte  is

P ij a P a E f" "T g a rT h@Z A PZ b b  Z b

rZ hT @Z A;
Q U U U S

@eFISA

where
P

PZ a

E f "Z " T g Z

T T T R

PZ1 : H : : H : PZnij

@eFITA

nd
PZk

IIP
P

a a

E f"Zk "T k g a E fR Z
P

Qij R k

ij Sk

Q S:

~ i uk ~ j uk

Q S

@~ ik AT @~ jk AT g u u @eFIUA

ustituting from iqutions @eFIQA nd @eFITA in iqution @eFISA yields

P b

a a a

nij

k=1 P nij
P R

rT k h@Z A PZk rZk hT @Z A Z

rT k hp Z R T h k=1 rZk 
Ppp Pp Pp P

@Z A S P r hT @Z A r hT @Z A Z Z Zk  @Z A k k p
Q S:

@eFIVA

por ;  P fp; g eh of the previous suEmtries n e written s

P

a a

nij

k=1 nij  k=1

rT k h @Z APZk rZk hT @Z A Z 
T @r~ ik h A Qij @r~ ik hT A u  k u ij T C @r~ jk h A Sk @r~ jk hT A u  u


@eFIWA

where we sustituted from iqF @eFIUA nd the reltion

T T rT k h @Z A a r~ ik h @Z A r~ j h @Z A : Z u u
k

IIQ sn order to derive the expressions for the ovrine suEmtries we ompute the following quntities from iqutions @QFITA nd @QFIVAX
T r~ ik hp a u
2n ij

T r~ ik h 9 rI qk J @Pkij A ; u T I q J @P ij A Rij ; T r~ ik h 9 r k k u
1 1

r~ jk hp a u

m=1 2n ij

ij @P m A

31
1

@Pkij A ;
1

@eFPHA
1

m=1

ij @P m A

31
1

@Pkij A Rij ;

@eFPIA @eFPPA @eFPQA

with
ij Pk

qk rT

ij T a Qij C Rij Sk Rij ; k u a Rij ~ jk ; nij T a qk J @Pkij A Jqk :

1

k=1

sn iqutions @eFPPA nd @eFPQA we employed the pproximtion mde in iqF @eFIHAF he interested reder is referred to RR for the detils of these derivtionsF fy sustituting iqutions @eFPHA through @eFPQA in iqution @eFIWA the suEmtries of the ovrine mtrix for the estimted motion vetor T a pT ij in iqF @eFIVA n ij now e omputedF he nl expressions re given y iqutions @QFIUA{@QFPPAF

IIR

Appendix B
Implementation Details
B.1 Obstacle Constraints

e requirement for ny useful motion plnning lgorithm is tht the pths it genertes re correctD iFeFD the pths re ontined wholly within the freespeF sn other wordsD the lgorithm should not generte pths tht interset ostlesF o mintin orretnessD the optimiztion method must hve some knowledge of ll the ostles tht the root knows outF e hoose to inorporte the ostles s onstrints in the optimiztion proessF por simultion purposesD we ssume tht ll ostles re irlesF his ws hosen s trdeo etween simpliity of implementtion nd uryF roweverD this ssumption does not prelude omplex situtionsF e union of irles n well pproximte virtully ny ostleF ine the pth is speied s numer of wypointsD one onstrint is tht none of these wypoints lies inside ny ostleF edditionllyD if we ssume tht the root moves in stright lines etween onseutive wypointsD there must e n dditionl onstrint tht prevents these line segments from interseting the ostlesF pigure fFI shows simple exmple with one ostleF he enter of the ostle is t lotion xc with rdius RF he root steps from wypoint xi to xiD then on to xi F he onstrint is s followsX ih pth segment nnot interset the irleF eny numeril optimiztion sheme will require tht the onstrint e written s c@xA < HF c@xA is olumn vetorD where eh row of c desries single onstrint eqution s funtion of xD the vriles eing optimizedF pirst onsider single line nd single irleF he irle is dened s oveD nd the
1 +1

xi

IIS
xi+1

R xi1

xc

pigure fFIX e portion of the pth where the root psses y n ostleF line is dened y the points x nd x F he prmetri eqution for line is
1 2

x a x1 C t@x1 x2 A;

@fFIA

where t is the prmeterD nd x is ny point on the lineF he point on the line tht is losest to the irle is lso losest to the enter of the irleF gll this point losest to the irle xdD nd ll the distne from the enter of the irle to this point dcF ee pigure fFI for grphil explntionF he point xd will lie on line tht psses through xc nd is

x2 x1 xd dc xc

pigure fFPX etup of single line nd irleF perpendiulr to the line dened y x nd x F e n write this s the dot produt of these lines eing equl to zeroF o onstrin xd to the line dened y x nd x D we n use @fFIAF his gives us three equtions nd three unknowns @the two omponents of the
1 2 1 2

IIT point xd nd the prmeter tdAF hese equtions re @x x A @xc xdA a H; x C td @x x A a xd :

2 1 1 2 1

@fFPA @fFQA

iqution @fFQA is tully two seprte equtionsD one for eh omponentF st is not too diult to solve these three equtions for xd nd tdF xote tht we will end up with equtions for xd nd td s funtions of x D x D nd xcF his is extly wht we wntD s x nd x re the vriles of the optimiztion prolem we re trying to solveF he eqution for td is
1 2 1 2

td a

x1 @IA@x1 @IA x2 @IAA C x1 @PA@x1 @PA x2 @PAA C @x1 @IA C x2 @IAAxc @IA C @x1 @PA C x2 @PAAxc @PA : @x1@IA x2@IAA2 C @x1@PA x2@PAA2

snspetion of this eqution leds to slightly more ompt formX

td a x1 @x1 x2 A C @x2 x1 A xc : kx1 x2k2

@fFRA

@fFSA

yne td is knownD it is trivl to sustitute it k into @fFQA nd nd xdF

B.1.1 The Constraint Equations

xow tht we hve funtions for the vriles of interestD we n write down wht the onstrints themselves reF sf dc > RD then the line segment @in ft the entire lineA does not interset the irleF sf on the other hnd dc < RD the line does interset the irleD ut it is still possile tht the segment tht we re out does notF edding in the onstrint tht the segment endpoints must lie outside the irleD iFeF
d@x2 ; xc A > R; d@x1 ; xc A > R;

@fFTA @fFUA

where d@; A is the iuliden distne etween rguments further redues the ses where the segment intersets the irleD ut not yet ompletelyF sf the prmeter td lies etween zero nd oneD then the losest point on the line is lso on the segment of interestF o restriting td to e greter thn one or less thn zero will fully onstrin the segment to e

outside the irleF sn summry there re three seprte sesD one of whih must e stised for the line segment to lie outside the ostleF he rst is tht the entire line lies outside the irleX
dc > R:

IIU

@fFVA

he seond nd third ses re tht the line intersets the irleD ut oth of the segment endpoints lie outside the irle nd the losest point on the line to the irle enter does not lie etween the segment endpointsF here re two seprte ses hereD the rst eing
d@x1 ; xc A > R; d@x2 ; xc A > R; td <

H:

@fFWA

nd the seond se is
d@x1 ; xc A > R; td > d@x2 ; xc A > R;

I:

@fFIHA

hese sets of onstrint equtions will onstrin the segment to lie outside the irleF fut we only need one of the three sets to e true t ny timeF sf @fFVA is trueD then we do not need to hek @fFWA nd @fFIHAF sn logil senseD we wnt to y the onstrints togetherD nd hek tht the outome is trueF o it will e neessry to write this group of seven equtions s single onstrintF gondensing this group of exh9s nd y9s into single eqution n e done with mx nd min opertionsF por exmple if we wnt to require tht
a > H exh b > H;

@fFIIA

then to write this s one onstrint c@xA > HD it eomes min@a; bA > H: @fFIPA

IIV sf the onstrint must e speied s c@xA < HD mx@a; bA < H: yn the other hnd if the onstrint is
a > H y b > H;

@fFIQA

@fFIRA @fFISA

we n write it s

mx@a; bA > H

to express the onstrint s c@xA > HF o write our spei onstrints s one equtionD we strt with R dc < H y @fFITA max@R d@x ; xc A; R d@x ; xc A; td A < H y max@R d@x ; xc A; R d@x ; xc A; I td A < H ; here the equtions hve een reorgnized to e of the form c@xA < HF wtl9s fmincon requires the nonEliner inequlity onstrints to e written in this formF o nlly omine them ll togetherD we hve min@c ; c ; c A < H; @fFIUA
1 2 1 2 1 2 3

where
c1 c2 c3

a a a

R dc ; max@R d@x1 ; xc A; R d@x2 ; xc A; td A; max@R d@x1 ; xc A; R d@x2 ; xc A; I td A:

@fFIVA @fFIWA @fFPHA

IIW

Appendix C
Shortest Path Properties
vet @tA e smooth pth in the free ongurtion spe p F yur gol in this setion is to estlish the following neessry ondition for the shortest pth in freespe ontining ojets with smooth oundriesX Necessary Condition: Let p & Rk be bounded by polygonal c-obstacles. Let @tA be
the shortest path in

k@tAk a I for all t.

connecting qinit to qgoal . Let the path be parametrized such that

1. If @tA passes at t

a H through an interior point of the freespace, @tA must be a straight line in a neighborhood about t a H. a H through a vertex d of a c-obstacle gfi, @HA a d , then the path's acceleration, @HA, must be antipodal to the generalized gradient of the  distance function dst@d; gfi A at d ,
0 0 0

2. If @tA passes at t

@HA a  for some  P @dst@d0 ; gfi A;  dst@x; S A is the distance between the point x and the set S , dened as d@x; S A a min kx qk:
q PS

where 

! H is a non-negative scalar, @f @A is the generalized gradient of f @A, and

@gFIA

Remark:

st n e shown tht dst@d; A is ontinuous nd vipshitzF gonsequently it is dierentile lmost everywhereF sn the interior of its grdient vetor is lwys zeroF et points d outside tht hve unique losest point d in its grdient

IPH is rdst@d; A a @d dA=kd dkF et points on the oundry of its generlized grdient is the onvex omintion of zero nd the outwrd normls to the smooth pthes omprising the oundry in smll neighorhood out this point IPF he si tool used y lulus of vritions is the following vrition of pthX vet @tA X a; b 3 p & Rk e smooth pthF e variation of @tA is smooth mp @t; sA X a; b @; A 3 p suh tht @t; HA a @tA @ > H is some xed positive onstntAF st is xed endpoint variation if @a; sA a @aA nd @b; sA a @bA for ll s P @; AF
Denition 1.

pigure gFI shows the imge of typil vritionF st is smooth @not neessrily homeomorphi3A imge of the retngle a; b @; A & R D whose kone urve is @tAF por xed sD the pth prmetrized y tX
2

s @tA 

@t; sA @s held onstntA

@gFPA

is smooth pth tht runs prllel4 to @tAF es s is vriedD olletion of urves s@tA is genertedF his fmily n e interpreted s urve of urves4 in suitly dened @IE dimensionlA mnifold whose points4 re urves onneting @aA to @bAF sn this mnifold  @sA  s @tA is smooth pth pssing through the point orresponding to the kone urve @tA t s a HF
@dst(d,CB) (t) .. ( 0) s a ( b)

b t

( a)

pigure gFIX e xed endpoint vritionF he toin mtrix of @t; sA is the k P mtrix D @t; sAF e will need the following

IPI two coordinate vector elds long @t; sAD whih re extly the olumns of D @t; sAX
E1 @t; sA E2 @t; sA
1

I @t;  D @t; sA R S a @ @t sA ;
 D

P Q

H P Q @t; @t; sA RHS a @ @s sA : I

@gFQA @gFRA

he vetor eld E @t; sA is tngent to the urve s@tA @s onstntAF sn prtiulrD t s a HD

E1 @t; HA a @tA
2

@gFSA

is the tngent long @tAF he other vetor eldD E @t; sAD is tngent to urves resulting from tring @t; sA for xed tF hey run in diretion trnsversl to the kone urve @tAF sn prtiulrD t s a HD E @t; sA gets speil nmeX
2

X @tA a E2 @t; HA;

@gFTA

nd is lled the variation vector eldF xote tht for xed endpoint vrition X @aA a X @bA a HF sn generl the formul for the length of smooth pth s@tA X a; b 3 Rk @using the iuliden normA is given y
l @ s A

a a

b b

ks@tAkdt kE @t; sAkdt:

1

@gFUA @gFVA

hen evluted on fmily of urvesD smoothly prmetrized y sD l eomes smooth relEvlued funtion of sD ssigning to every s in @; A the length of s@tAF he si ide of the lulus of vrition is s followsX sf @tA is the shortest pth in the d fmily s@tAD s a H must e sttionry point of l@sAF ht isD the derivtive ds s l@sA d must e zeroF vet us now ompute the derivtive ds l@sA t s a HF pirst we hve
=0

d  l @ A a ds s=0 s

d  kE @t; sAkdt: ds s=0 1

@gFWA

sing the hin ruleD



IPP
 

 d d  kE1@t; sAk a kE @It; sAk  E1@t; sA ds  E1@t; sA:   ds s=0 1 s=0 s=0

@gFIHA

he oeent is unity sine kE @t; HAk a IF xow oserve tht

1

d  E @t; sA ds s=0 1

a a a

d @  @t; sA ds s=0 @t  @ @  @t; sA @t @s s=0 d X @tA: dt

1

@gFIIA @gFIPA @gFIQA

ustituting X @tA for



d  ds s=0 E1

@t; sA nd @tA for E @t; HA in the integrl gives

b b

d  l @ A ds s=0 s

a a

X @tA @tAdt
d @X A X  dt  dt
= =

@gFIRA


@gFISA
b

a @X Ajt b @X Ajt a
 b

@X A dt: 

@gFITA

he rst two terms vnish for xed endpoint vritionF he resulting formulD
d  l @ A a ds s=0 s
a

@X A dt; 

@gFIUA

is lled the rst variation of the path-length integralF he neessry ondition b @X A dt a H; 

a

@gFIVA

it n e shownD is stised y the shortest pth @tA only if the integrnd X @tA @tA is  zero for ll tF vet us now sketh how this implies the neessry ondition stted t the eginningF vet @tA e the shortest pthF pirst let us see why @HA a H @iFeFD stright lineA out  every interior pointF vet @HA e n interior point of p F sf @HA Ta HD it is possile to  onstrut vrition of the form @t; sA a @tA s@tA@HA;  @gFIWA

IPQ where @tA is smooth relEvlued funtion tht is extly unity t t a HD nd deys smoothly to zero wy from t a H @ bump function AF xote tht t t a H we hve tht
X @HA @HA a k@HAk2 < H:  

@gFPHA

his mens tht @tA hs neighoring urveD s@tA for some smll xed s > HD whose length is smller thn the length of @tAF he sme ide pplies when @HA a d is oundry pointF sf @HA is not ntipodl to the generlized grdient @ dst@d ; AD it is possile to onstrut  vrition @t; sA of @tAD whose vrition vetor eld X @tA points into the hlfspe of diretions pointing wy from @HAD so tht 
0 0

X @HA @HA < H: 

@gFPIA

his implies the existene of neighoring urve in p of shorter lengthF

IPR

Appendix D
Bounded Uncertainty Freespace
ht is the set of points from whih the root n reh the gol from with Ug < c he following setions show tht this set is oundedF he ext shpe of the oundry is not determinedD ut ound is pled on the extremes of this regionF
D.1 Goal Only

pirst onsider the se where there re no lndmrksF he root hs to rely on its odometry lone for loliztionF sn this se the root9s unertinty will ontinue to inrese s it movesF por the simple point root model tht is ssumed hereD nd under the ssumption tht the vrine of the root veloity is proportionl to the veloityD iFeFD v a V D it is known tht @xR @k C IA xR @kAA H S : @hFIA PRR @k C I=kA a PRR @k=kA C  R H @yR @k C IA yR @kAA
2 2 2

sn the est se the root will strt out with perfet estimte of its poseD iFeFD PRR @HA a H F sf the initil ovrine is nonEzeroD then the ounds will e smllerF4 en expliit limit n e pled on how lrge the root9s ovrine n grow in this se efore it termintes with filureF equiring the root to get to within  of the gol trnsltes to limit on the mximum size of the ovrine mtrixF purther ssuming tht the root9s true position will lwys e within the Q ellipse of the position estimte error ovrineD
2 2

IPS

3

max

pigure hFIX wximum size of ovrine ellipse to t inside irle with rdius of F then n upper limit on the mximum vrine existsX Qmax < : @hFPA

qrphillyD the inequlity looks like pigure hFIF he Q ovrine ellipse hs to t inside the irle of rdius F sf the root tkes one step from its strting lotion to the golD its ovrine fter tht step will e P Q @gx xR @HAA H S; PRR @I=HA a  R @hFQA H @gy yR @HAA
2 2 2

where g a gx gy is the gol positionF he length of the xes of the ovrine ellipse re proportionl to the eigenvlues of the ovrine mtrixD nmelyD
i a i ;
p

iT

@hFRA

where i is the vrine long the ith diretionD nd i is the orresponding eigenvlueF sn this se the ovrine mtrix is digonlD so the eigenvlues re just the digonl elements

IPT

2 3

pigure hFPX et of points from whih the gol n e rehed with max < F he set is the drkened oxF themselvesF hereforeD
max a  mx @jgx xR @HAj; jgy yR @HAjA :

@hFSA

iqution @hFPA n e reEwritten s Q mx @jgx xR @HAj; jgy yR @HAjA mx @jgx xR @HAj; jgy yR @HAjA
2 3

< ;  < Q :
h i

@hFTA @hFUA

hese onstrint equtions n e interpreted s requiring xR @HA yR @HA to lie inside  squre entered t g with side length  F pigure hFP shows this grphillyF
D.2 Goal and One Landmark

xow onsider the se where there is one lndmrk in the environmentF ixtension of these results to the se of multiple lndmrks n e found in lter setionF eginD this is only pling ounds on the set of points the root ould reh the gol fromD not expliitly nding the setF his prolem n e roken down into nding susets of the entire setF pirstD one n nd the set of points from whih the root n reh the gol y relying on odometry loneF his set ws found in etion hFIF he set of points from whih the root n strt

IPU

2R 3

2 3

pigure hFQX e ound on the set of points from whih the gol ould e rehed with max < F nd reh the lndmrk relily is lso neededF sf the root strts too fr wy from the lndmrk @or the golAD it will eome lost4 efore it even rehes the lndmrk nd hs hne to lolize itselfF he lndmrk n e thought of s n intermedite golF his is nlgous to the se of rehing the golD ut insted of requiring tht the root get within D the requirement is tht the root get within R of the lndmrkF R is the root9s sensor rngeD nd it must get t lest tht lose to the lndmrk to e le to use it for loliztionF he union of these two sets ontins the set of ll possile points tht the root ould strt nd reh the gol fromF sf the root strts nywhere outside this unionD it will not e le to reh either the gol or the lndmrk efore its unertinty exeeds level where it n no longer gurntee tht it will nd the gol or the lndmrkF o nd the set of points from whih the root n reh the lndmrk with unertinty < RD  is repled with R nd g is repled with L @the lndmrk positionA in iqution @hFUAF his nlysis yields the result mx @jLx xR @HAj; jLy yR @HAjA < QR :  pigure hFQ shows the union of these two setsF @hFVA

IPV

2R 3

L1 L3
2 3

2R 3

g
2R 3

L2

pigure hFRX nion of the multiple setsF

D.3 Goal and Multiple Landmarks

he se of multiple lndmrks is strightforwrd extension of the singleElndmrk seF he union of ll of the sets dened y eh lndmrk nd the set dened y the gol is tkenF sf the root strts nywhere outside this unionD then it nnot reh the gol or lndmrk without exeeding n unertinty threshold tht gurntees tht it will e le to see the lndmrk @golA when it thinks it hs rrived thereF pigure hFR shows n exmple of this set with three lndmrksF xote tht the susets dened y n individul lndmrk need not e the sme sizeF sf for some reson the root n sense one lndmrk t dierent rnge thn notherD then the oxes ould e dierent sizesF eginD this set is not the set of points tht the gol n e rehed from with Ug < D ut it does ontin itF

IPW

Bibliography
I wFhF edmsF vidr designD useD nd lirtion onepts for orret environmentl detetionF IEEE Trans. Robotics and AutomationD IT@TAXUSQ{UTID heF PHHHF QFID SFQ P wFhF edms nd FtF roertF he interprettion of phse nd intensity dt from ewg light detetion sensor for relile rngingF Int. J. of Robotics ResearchD IS@SAXRRI{RSVD ytF IWWTF QFID QFPFID QFQFID QFQFQ Q wFgF emnnD F foshD wF vesureD F wyllylD nd wF iouxF vser rngingX ritil review of usul tehniques for distne mesurementF Opt. Eng.D RH@IAXIH{IWD tnF PHHIF QFID QFQFI R hF fllrdF qenerlizing the rough trnsform to detet ritrry shpesF RecognitionD IQ@PAXIII{IPPD IWVIF QFS
Pattern

S yF fengtsson nd eFtF ferveldtF voliztion in hnging environments| estimtion of ovrine mtrix for th shg lgorithmF sn Proc. IEEE/RSJ Int. Conf. on Intelligent Robots and SystemsD pges IWQI{UD wuiD rwiiD ytF PHHIF QFI T FF flkmnF wultiple hypothesis trking for multiple trget trkingF IEEE Aerospace and Electronic Systems MagazineD IW@sssue ID rt PAXS{IVD tnF PHHRF PFPFPFI U hFwF flei nd vFF uellingF hortest pths in dynmi unertin dominF sn Proc. of the IJCAI Workshop on Adaptive Spatial Representations of Dynamic EnvironmentsD IWWWF IFP V eF friggsD gF hetweilerD hF hrsteinD nd eF ndenergEodesF ixpeted shortest pths for lndmrkEsed root nvigtionF sn Proc. of the Fifth Int. Workshop on the Algorithmic Foundations of RoboticsD heemer PHHPF IFP

IQH W eF friggsD gF hetweilerD hF hrsteinD nd eF ndenergEodesF ixpeted shortest pths for lndmrkEsed root nvigtionF Intl J. of Robotics ResearchD PQ@UEVAXUIU{ UPVD tulyEeugust PHHRF IFP IH rF ghoset nd tF furdikF ensor sed plnningD prt sX he generlized oronoi grphF sn Proc. IEEE Int. Conf. on Robotics and AutomationD volume PD pges ITRW { ITSSD wy IWWSF SFR II rF ghoset nd tFF furdikF ensor sed explortionX the hierrhil generlized oronoi grphF Int. J. of Robotics ResearchD IW@PAXWT{IPSD pe PHHHF IFID IFP IP pF rF glrkeF Optimization and nonsmooth analysisF ileyD IWVQF g IQ F rF gormenD gF iF veisersonD F vF ivestD nd gF teinF Introduction to Algorithms, Second EditionF ws ressD PHHIF IFP IR sFtF goxF flnhe|n experiment in guidne nd nvigtion of n utonomous root vehileF IEEE Trans. on Robotics and AutomationD PXIWQ{PHRD IWWIF IFPD QFID QFI IS FyF hud nd FiF rrtF se of hough trnsform to detet lines nd urves in pituresF Communications of the ACMD IS@IAXII{ISD IWUPF QFS IT hF poxD F furgrdD nd F hrunF etive mrkov loliztion for moile rootsF Robotics and Autonomous SystemsD PSXIWS{PHUD IWWVF IFP IU tF qonzlez nd F qutierrezF woile root motion estimtion from rnge sn sequeneF sn Proc. IEEE Int. Conf. on Robotics and AutomationD pges IHQR{WD xew orkD xD eprF PHEPS IWWUF IFPD QFID QFI IV eFiF tohnson nd eF wiguel n wrtinF wotion estimtion from lser rnging for utonomous omet lndingF sn Proc. IEEE Int. Conf. on Robotics and AutomationD pges IUVV{IUWSD n prnisoD geD eprF PR{PV PHHHF QFU IW FiF ulmnF e new pproh to liner ltering nd predition prolemsF Transactions of the ASME{Journal of Basic EngineeringD VP@eries hAXQS{RSD IWTHF IFP PH sF umonD iF imonD nd iF ivlinF ngentugX e rnge sensor sed nvigtion lgorithmF Int. J. of Robotics ResearchD IU@WAXWQR{WSQD eptF IWWVF IFID IFPD PFI

IQI PI sF umonD iF imonD nd iF ivlinF ngeEsensorEsed nvigtion in threeE dimensionl polyhedrl environmentsF Int.l J.l of Robotics ResearchD PH@IAXT{PSD tnE ury PHHIF RFS PP F uirurjn nd F frEhlomF roilisti dt ssoition tehniques for trget trking in lutterF Proc. of the IEEED WP@QAXSQT{SSUD wr PHHRF PFPFPFI PQ eF vmert nd F prihrdF vndmrkEsed sfe pth plnning for rElike rootsF sn Proc. IEEE International Conference on Robotics and AutomationD PHHHF IFP PR FiF vrson nd tFvF gstiF Principles of Dynamic ProgrammingF IWUVF SFQ PS tFgF vtomeF IWWIF RFI
Robot Motion PlanningF

uluwer edemi ulishersD fostonD weD

PT FvF vuhF Theory and Experiments in Autonomous Sensor-Based Motion Planning with Applications for Flight Planetary MicroroversF hh thesisD gliforni snstitute of ehnologyD IWWWF IFP PU FwF vlle nd FeF ruthinsonF en ojetiveEsed frmework for motion plnning under sensing nd ontrol unertintiesF Int. J. Robotics ResearchD IU@IAXIW{RPD tnF IWWVF IFP PV FwF vlle nd F hrmF yn motion plnning in hngingD prtil preditle environmentsF Int. J. Robotics ResearchD IT@TAXUUS{VHSD heemer IWWUF IFP PW eF vzns nd tFgF vtomeF vndmrk sed root nvigtionF IQ@SAXRUP{SHID wy IWWSF IFP
AlgorithmicaD

QH eF vzns nd tFgF vtomeF wotion plnning with unertintyD lndmrk pprohF Articial IntelligenceD UT@IEPAXPVU{QIUD tuly IWWSF IFP QI tF veonrd nd rF hurrntEhyteF woile root loliztion y trking geometry eonsF IEEE Trans. on Robotics and AutomationD U@QAXQUT{QVPD tune IWWIF IFID IFP QP F viu nd wFeF odriquesF eurte registrtion of strutured dt using two overE lpping rnge imgesF sn Proc. IEEE Int. Conf. on Robotics and AutomationD pges PSIW{PRD shington hFgFD wy II{IS PHHPF QFU

QQ eF vogothetisF IWWUF IFP

IQP
EM Algorithms for State and Parameter Estimation of Stochastic Dy-

namical SystemsF

hh thesisD niversity of welourneD itoriD eustrliD eugust

QR eF vogothetisD eF sskssonD nd F tF ivnsF en informtion theoreti pproh to oserver pth design for eringsEonly trkingF sn Proc. of the 36th Conference on Decision and ControlD pges QIQP{QIQUD n hiegoD geD eD heemer IWWUF ghgF IFP QS pF vorussiD eF wrigoD nd eF fihiF yptiml explortory pths for moile roverF sn Proc. of the IEEE Int. Conf. on Robotics and AutomationD PHHIF IFP QT pF vu nd iF wiliosF qlolly onsistent rnge sn lignment for environment mpE pingF Autonomous RobotsD RXQQQ{QRWD IWWUF IFPD QFID QFPFPD QFTFI QU pF vu nd iF wiliosF oot pose estimtion in unknown environments y mthing Ph rnge snsF J. of Intelligent and Robotic SystemsD PHXPRW{PUSD IWWUF IFPD PFPFPFID QFID QFID QFPFPD QFRD QFT QV FtF vumelsky nd eFeF tepnovF thEplnning strtegies for point moile utomE ton moving midst unknown ostles of ritrry shpeF AlgorithmicaD PXRHQ{RQHD IWVUF IFID IFP QW F F wyekF IWUWF PFPFI
Stochastic Models, Estimation, and ControlF

edemi ressD snFD

RH F woutrlier nd F ghtilF tohsti multisensory dt fusion for moile root lotion nd environment modellingF sn Proc. International Symposium on Robotics ResearchD pges VS{WRD okyoD eugust IWVWF IFP RI F ster nd tF F furdikF eighted line tting lgorithms for moile root mp uilding nd eient dt representtionF sn Proc. IEEE Int. Conf. on Robotics and AutomationD ipeiD iwnD eptF PHHQF QFQFPD QFS RP FF sterD uFvF uriehumD FsF oumeliotisD nd tFF furdikF eighted rnge sensor mthing lgorithms for moile root displement estimtionF sn Proc. IEEE Int. Conf. on Robotics and AutomationD shingtonD hFgFD wy PHHPF IFID PFPFPFID QFTFR

IQQ RQ F ezeiD tF quivntD tF xietoD nd iF wF xeotF imultneous informtion nd glol motion nlysis @sqwe4A for rElike rootsF sn Proc. of the IEEE Int. Conf. on Robotics and AutomationD PHHRF IFP RR FsF oumeliotisF hense rnge feture mthingX weighted roE ttionl displement estimtionF ehnil reportD gFsFFD PHHIF http://robotics.caltech.edu/$ stergios/tech reports/tr wlsm orientation.pdfF eFQ RS FsF oumeliotis nd tF furdikF tohsti loningX e generlized frmework for proessing reltive stte mesurementsF sn Proc. IEEE Int. Conf. on Robotics and AutomationD pges IUVV{IUWSD shington hFgFD wy IIEIS PHHPF QFID QFU RT xF oyD F furgrdD hF poxD nd F hrunF gostl nvigtion|moile root nviE gtion with unertinty in dynmi environmentsF sn Proc. of the IEEE Int. Conf. on Robotics and AutomationD IWWWF IFPD TFP RU xF oy nd F hrunF gostl nvigtion with moile rootsF sn Proc. of Conference on Neural Information Processing SystemsD IWWWF IFPD TFP RV FF mithD xF xndhkumrD nd gFrF ghienF yjet motion nd struture reovery for rooti vision using snning lser rnge sensorsF IEEE Trans. on Robotics and AutomationD IQ@IAXUR{VHD peF IWWUF QFU RW FgF mith nd F gheesemnF yn the representtion nd estimtion of sptil unerE tintyF Int. J. of Robotics ResearchD S@RAXST{TVD IWVTF IFP SH FgF mithD wF elfD nd F gheesemnF istimting unertin sptil reltionships in rootisF Autonomous Robot VehiclesD pges ITU{IWQD IWWHF IFID IFP SI FpF tengelF Optimal Control and EstimationF hoverD IWWRF SFQ SP F khshi nd fFuF qhoshF wotion nd shpe identition with vision nd rngeF IEEE Trans. on Robotics and AutomationD RU@VAXIQWP{TD eugF PHHPF QFU SQ F hrunF fyesin lndmrk lerning for moile root loliztionF Machine LearningD Q@IAXRI{UTD ytF IWWVF IFQ SR F hrunF rtile lters in rootisF sn Proceedings of the 17th Annual Conference on Uncertainty in AI (UAI)D PHHPF IFP

IQR SS F hrunD hF poxD nd F furgrdF e proilisti pproh to onurrent mpping nd loliztion for moile rootsF Machine LearningD QIXPW{SQD IWWVF IFI ST xF rwny nd F frfootF yptimized motion strtegies for oopertive loliztion of moile rootsF sn Proc. of the IEEE Int. Conf. on Robotics and AutomationD PHHRF IFP SU eFgF itorinoD F ivesD nd tF forrellyF e reltive motion estimtion y omining lser mesurement nd sensor sed ontrolF sn Proc. IEEE Int. Conf. on Robotics and AutomationD pges QWPR{WD shington hFgFD wy II{IS PHHPF QFID QFPFQ SV gF e nd tF forensteinF ghrteriztion of PEd lser snner for moile root ostle negotitionF sn Proc. IEEE Int. Conf. on Robotics and AutomationD pges PSIP{PSIVD shington hFgFD wy II{IS PHHPF QFTFR