Robust autonomous robot localization using interval analysis

HAL Id: hal-00844915

https://hal.archives-ouvertes.fr/hal-00844915

Submitted on 16 Jul 2013

HAL is a multi-disciplinary open access archive for the deposit and dissemination of sci- entific research documents, whether they are pub- lished or not. The documents may come from teaching and research institutions in France or abroad, or from public or private research centers.

L’archive ouverte pluridisciplinaire HAL, est destinée au dépôt et à la diffusion de documents scientifiques de niveau recherche, publiés ou non, émanant des établissements d’enseignement et de recherche français ou étrangers, des laboratoires publics ou privés.

analysis

Michel Kieffer, Luc Jaulin, Eric Walter, Dominique Meizel

To cite this version:

Michel Kieffer, Luc Jaulin, Eric Walter, Dominique Meizel. Robust autonomous robot localization using interval analysis. Reliable Computing Journal, 2000, 3 (6), pp.337-361. �hal-00844915�

Robust Autonomous Robot Loalization

Using Interval Analysis

MICHELKIEFFER 1

,LUCJAULIN 1;2

,

ERICWALTER 1

*

ANDDOMINIQUEMEIZEL 3

1

LaboratoiredesSignauxetSystemes,CNRS-Supele

Plateau deMoulon,91192 Gif-sur-Yvette,Frane

fkieer,jaulin,walterglss.supele.fr

2

onleavefromLaboratoired'IngenieriedesSystemesAutomatises, Universite d'Angers,

2bdLavoisier,49045Angers,Frane

jaulinbabinet.univ-angers.fr

3

HEUDIASYC,CNRS,Universite deTehnologiedeCompiegne,

B.P.20529,60205Compiegne,Frane

meizelhds.univ-ompiegne.fr

Editor:

Abstrat. Thispaper dealswiththe determination of the position and orientation of a mo-

bilerobot fromdistane measurementsprovided bya belt of onboardultrasoni sensors. The

environmentisassumedtobetwo-dimensional,and a mapofitslandmarksisavailable tothe

robot. Inthis ontext, lassial loalization methodshavethree main limitations.First, eah

datapointprovidedbyasensormustbeassoiatedwithagivenlandmark.Thisdata-assoiation

stepturnsoutto beextremely omplexandtime-onsuming,and itsresultsanusuallynotbe

guaranteed.Theseondlimitationisthatthesemethodsarebasedonlinearization,whihmakes

them inherently loal. The thirdlimitationistheir lakof robustness tooutliers due, e.g., to

sensormalfuntionsoroutdatedmaps.Byontrast,themethodproposedhere,basedoninterval

analysis,bypassesthedata-assoiationstep,handlestheproblemasnonlinearandinaglobalway

andis(extraordinarily)robusttooutliers.

Keywords: IntervalAnalysis- Identiation -StateEstimation- Outliers- BoundedErrors-

Robotis.

1. Introdution

Robots are artiulatedmehanialsystems employed for tasks that may be dull,

repetitiveandhazardousormayrequireskills orstrength beyondthoseof human

beings. They rst appeared as manipulating robots with their base rigidly xed,

performing simple and well dened elementary tasks in a ontrolled workspae.

Sine then, muh of the researhin robotis hasbeendevoted to inreasingtheir

autonomy,e.g.,by addingsensors,mobility anddeisionapability. Mobile robots

maytakevariousformsdependingonthetaskandenvironment.Tobeautonomous,

theymustbeabletoestimate theirpresentstatefromavailablepriorinformation

andmeasurements.

Theproblemtobeonsideredhereistheautonomousloalizationofarobotsuh

asthat desribed by Figure 1from distane measurements provided by a belt of

*

onboardexteroeptivesensors. Here,ultrasonisensorsareused,whihareknown

to beheap but impreise. Other typesof sensors providing rangedata ould be

onsidered, with the samemethodology. Theenvironmentis assumed to be two-

dimensional(althoughathree-dimensionalextensionposesnoprobleminpriniple),

and amapof itslandmarks isavailableto therobot. Nospeial beaonsneed to

beintroduedin theenvironmenttofailitateloalization.

Figure1. Robuter mobilerobotbyRobosoft.

Inthis ontext, lassialloalization methods [4℄, [2℄, [17℄, [6℄, [18℄, [21℄and [5℄

havethree main limitations. First, eah data pointprovided by an exteroeptive

sensormustbeassoiatedwithagivenlandmark.Thisdata-assoiationstepturns

outtobeextremelyomplexandtime-onsuming,anditsresultsanusuallynotbe

guaranteed.Theseondlimitationisthatthesemethodsarebasedonlinearization,

whihmakestheminherentlyloal. Thethirdlimitationistheirlakofrobustness

to outliers due, e.g., to sensor malfuntions or outdated maps. By ontrast, the

method proposedhere, whih is basedon bounded-error set estimation (see, e.g.,

[27℄, [22℄, [23℄and [20℄, and thereferenes therein),bypassesthe data-assoiation

step, handles the problem asnonlinearand in aglobal way(see also [19℄) and is

(extraordinarily)robusttooutliers.

Thispaperisorganizedasfollows.Theproblemisstatedin mathematialterms

in Setion 2. Setion 3desribes theelementarytests that will beused to loate

therobot. Extensiontointervalsand ombinationofthese testsareonsidered in

Setion4.Setion5desribesthealgorithmemployedtoharaterizethesetofall

values of theloalization parametersthat satisfy the tests hosen. Theresulting

methodology isillustrated on three tests asesin Setion 6, before drawingsome

2. Formulationof the problem

Computationwillinvolvetwoframes,namelytheworldframeWandaframeR,of

origin=(x

;y

)in W, tiedtothe robot. Theangle betweenRandW, denoted

by,orrespondstotheheadingangleoftherobot(seeFigure2). Pointsandtheir

oordinates will be denoted by lower-ase letters in W and by tilded lower-ase

lettersin R. Thus, apointm~ withoordinates(~x;y)~ in R willbedenoted by m

inW,with

m=

x

y

+

os sin

sin os

~ x

~ y

: (1)

Three parametersare to be estimated, namely the oordinates x

and y

of the

originofRinW andtheheadingangleoftherobot. Theyformtheonguration

vetor p=( x

;y

;) T

(Figure 2). Given some(possiblyverylarge)initial searh

box [p

0

℄ in onguration spae, robot loalization anbe formulated asthe task

ofharaterizingthesetS=fp2[p

0

℄ jt( p)holdstrueg, wheret(p)isasuitable

test or ombination of tests expressing that the robot ongurationis onsistent

withthemeasurementsandpriorinformation.

W

R

x_c y_c

µ c

Figure2. Congurationoftherobot.

2.1. Measurements

The robot of Figure 1 is equipped with abelt of n

s

onboard Polaroid ultrasoni

sensors (sonars). The position of the ith sensor in the robot frame R is ~s

i

=

(~x

i

;y~

i

): This sensor emits in aone haraterizedby its vertex~s

i

, orientation

~

i

and half-aperture ~ (Figure 3). As ~ is frame independent, ~ = . This one

willbedenotedbyC(~s

i

;

~

i

;~

i

). Thesensormeasuresthetimelagbetweenemission

and reeption of the wave reeted or refrated by some landmark. This time

lag is then onverted into a distane d

i

to some obstale. To take measurement

inaurayinto aount, eah data pointd

i

is assoiated with the interval [d

i

℄=

[d

i

(1

i );d

i ( 1+

i

) ℄, where

i

is the known relativemeasurementauray of

sensori. Thus,[d

i

℄isassumedtoontaintheatualdistanetothelosestreeting

landmarkintereptingatleastpartoftheithemissionone.

R

aj

bj-1

b_j aj+1

bj+1

aj-1

µi

:

°i

:

:yi

:

di

® di i

si

xi :

Figure3. Emissionone.

2.2. Priorinformation

Two types of prior information will be onsidered.The rst one is amap M =

f[a

j

;b

j

℄jj =1;:::;n

w

gof theenvironment,assumedto onsistofn

w

orientedseg-

ments whih desribe the landmarks (walls, pillars, et.). By onvention, when

goingfroma

j tob

j

,thereetingfaeofthesegmentisontheleft. Thehalf-plane

ajbj

situatedonthereetingsideofthesegment[a

j

;b

j

℄ isthereforeharater-

izedby

a

j b

j

= n

m2R 2

det

!

a

j b

j

;

!

a

j m

0 o

: (2)

Theseond typeofinformation(optional)istheknowledgeofasetdesribedby

3. Loalization tests

This setion enumerates various elementary tests that will be used to build the

globaltestt(p) employedtodeneS.

3.1. Data-assoiation test

Toestimate the robot onguration from range data provided by ultrasoni sen-

sors, it is of interest to build a test that heks whether agiven onguration is

onsistentwith thesedata, given theirimpreision. Forthis purpose, information

available in therobot frameRwillbetranslatedin theworldframeW. Consider

any ultrasoni sensor ofthe robot, with emissionone C

~s;

~

;~

(in this setion,

theindiesiandjwillbeomittedtosimplifypresentation). Foranygivenongu-

rationp=(x

;y

;) T

,CanbeequivalentlydesribedinWbyitsvertexs(p)and

bytwounit vetors

!

u

1

p;

~

;~

and

!

u

2

p;

~

;~

orrespondingtoits edges,given

by

!

u

1

= 0

os

+

~

~

sin

+

~

~

1

A

;

!

u

2

= 0

os

+

~

+~

sin

+

~

+~

1

A

: (3)

So one may write C =C(s;

!

u

1

;

!

u

2

) (omitting the dependeny in p;

~

and ~). By

onvention,

!

u

1 and

!

u

2

have been indexed so that

!

u

2

is obtained from

!

u

1 by a

ounterlokwiserotationof2~. Sine~isalwayslessthan =2;theonditionfor

anym2R 2

to belongto theemissiononeis

m2C(s;

!

u

1

;

!

u

2

),(det(

!

u

1

;

!

sm)0)^(det(

!

u

2

;

!

sm)0): (4)

Thealgorithmfortestingagivenongurationisbasedonthenotionofremote-

nessofasegmentfrom asensor,whih willnowbedened. Considerrstasingle

isolatedsegment[a;b℄ .Its remotenessfrom thesensors,assoiatedwiththeone

C( s;

!

u

1

;

!

u

2

),isdened as

r(s;

!

u

1

;

!

u

2

;a;b) =1ifs2=

ab

orif [a;b℄\C=;;

= min

m2[a;b℄\C k

!

smk otherwise.

(5)

The remotenessfuntion (5) is evaluated asfollows.Equation (2) is used rst to

hekwhether s 2

ab

: If this is so, minimization of k

!

smk over [a;b℄\C is at-

tempted.Thisrequirestakingdierentsituationsintoaount.Lethbetheorthog-

onalprojetionofsontotheline (a;b) . Ifh2[a;b℄\C, thenr(s;

!

u

1

;

!

u

2

;a;b)=

!

sh

. Tohekwhetherh2[a;b℄\C,withoutatuallyomputingit,onemayuse

thefollowingrelation:

h2[a;b℄\C , D

!

ab ;

!

sa E

0

^ D

!

ab;

!

sb E

0

^ D

!

ab ;

!

u E

0

^ D

!

ab;

!

u E

0

: (6)

If h 2= [a;b℄\C, the minimum distane is either innite (if [a;b℄\C = ;) or

obtained for oneof the extremities of the segment [a;b℄ \ C. Let h

1 and h

2 be

the intersetions of the line (a;b) with the lines (s;

!

u

1

) and (s;

!

u

2

): The set of

possibleends of [a;b℄\C is thus K =fa;b;h

1

;h

2

g: Therefore,if h2=[a;b℄\C;

thenr(s;

!

u

1

;

!

u

2

;a;b)iseitherinniteorequaltok

!

smk,forsomeminK . Forthe

exampleofFigure4,r(s;

!

u

1

;

!

u

2

;a;b)=

!

sb

:

b a

s

h h

h₂ 1

u2 u

1

Figure4. Remotenessofanisolatedsegment[a;b℄fromthesensors.

Atestofwhether anyelementofKbelongsto[a;b℄\C iseasilyderivedfrom(4).

Forv2fa;bg

v2C()(det(

!

u

1

;

!

sv)0)^(det(

!

u

2

;

!

sv)0): (7)

Byonstrution,h

i

2C\(a;b) ;onethus hasonlytohekwhether h

i

belongsto

[a;b℄;whihisequivalenttoprovingthath

i 2C

s;

!

sa

k

!

sak

;

!

sb

k

!

sbk

:Thus,fori=1;2,

h

i

2[a;b℄\C()( det(

!

sa;

!

u

i

)0)^

det

!

sb ;

!

u

i

0

: (8)

Finally,ifneitherhnoranyelementofK belongsto [a;b℄\C;then[a;b℄\C=;;

andtheremotenessisinnite.

AppendixApresentsafuntion,basedonthesetests,evaluatingr(s;

!

u

1

;

!

u

2

;a;b)

foranisolatedsegment[a;b℄ .

Remark. This versionof remoteness doesnot takeinto aount thefat that if

the inideneangle of theemitted waveis greater thana given angle(depending

on the nature of the landmark), no wave will return to the sensor. This ould

easily be taken are of by modifying the denition of remoteness so as to take

theinideneangleinto aount. Anotherphenomenonnotonsideredismultiple

reetion taking plae, for instane, in onave orners. Aountingfor multiple

notworthwhile. Aswill beseeninSetion4.3, amuhsimplerrouteistoonsider

suhmeasurementsasoutliers. }

In the normal situation where n

w

segments are present, the fat that a given

segment maynot be deteted,beauseit liesin the shadowof anotheroneloser

to the sensor, must be taken into aount. Let r

ij

(p) be the remoteness of the

jthsegment,takenasisolated,from theith sensor iftheongurationisp. This

remotenessis givenby

r

ij

(p)=r

s

i ( p);

!

u

1i

p;

~

i

;~

i

;

!

u

2i

p;

~

i

;~

i

;a

j

;b

j

: (9)

Theremotenessofthemapfrom theith sensoriftheongurationispisthen

r

i

( p)= min

j=1;:::;nw r

ij

(p): (10)

Themeasurementprovidedbytheithsensormaybeexplainedbyasegmentlying

ataproperdistane ifthefollowingtestis satised:

Test dat

i

( p): dat

i

( p)holds trueifandonlyifr

i

(p)2[d

i

℄.

3.2. Inroomtest

Assume that the map partitions the world into two sets, the interior, whih the

robotshould belong to,

P

int

= 8

<

: m2R

2

nw

X

j=1 arg

!

ma

j

;

!

mb

j

=2 9

=

;

; (11)

andtheexterior

P

ext

= 8

<

: m2R

2

nw

X

j=1 arg

!

ma

j

;

!

mb

j

=0 9

=

;

; (12)

wherearg

!

ma

j

;

!

mb

j

;theanglebetween

!

ma

j and

!

mb

j

,isonstrainedtobelong

to ℄ ;℄. The fat that is exludedimplies that theboundarybetweenP

int

andP

ext

belongstotheinterior. Figure 5illustrates asituation wherepartofthe

roomisforbiddenbysuitablyorientedinternalpolygons. Foreahsegment[a

j

;b

j

℄,

thearrowindiatesthediretionfroma

j tob

j

:Reallthatthereetingfaeison

theleftwhengoingfroma

j tob

j :

Ifm~ isanypointoftherobotwithoordinates(~x;y)~ inR,thenitsoordinatesm

inWevaluatedaordingto(1)dependontherobotongurationp=(x

;y

;) T

and the following test will make it possible to eliminate some ongurations for

whihitwouldnotbein P .

-12 12 -12

12

aj

bj

Figure5. Partitionoftheworld.Theinteriorisinwhite.

Test in room( m):

8

<

:

inroom( m)=1 if nw

P

j=1 arg

!

ma

j

;

!

mb

j

=2;

inroom( m)=0 otherwise.

Whenmistheprojetionofpontothexyplane,in room( m)willberewritten

asin room(p).

AsshowninSetion6,thistestwillontributetoeliminatingongurationsmore

eÆientlythanthedata-assoiationtestalone,onpurelygeometrialgroundsand

in the absene of any measurements. However, it is reliableonly when the map

and thepartition itindues are reliable.Evenwhen thisis notthease, this test

remains of interest, as it forms the basisfor the leg in test presented below and

stillappliable.

Remark. FititiousnonreetingsegmentsmaybeneededtodeneP

int andP

ext .

Theymaybetransparent(opendoorsandwindows),orabsorbing. Thereetivity

ofeahof thesesegmentsould betakeninto aountwith amoreelaborate de-

nitionofremoteness.With thedenition adoptedhere,suhsegmentsmayleadto

outliers,see Setion4.3. }

3.3. Legintest

Considerarobotongurationp=(x

;y

;) T

,theith robotsensors

i

,withoor-

dinates (~x ;y~) in R, and its assoiated interval measurement [d℄. Let bethe

point at adistane equalto thelowerbound d

i of[d

i

℄ from s

i

in thediretion of

emission

~

i

. Theoordinatesof

i

inW satisfy

i

=

x

y

+

os sin

sin os

0

~ x

i +os

~

i

d

i

~ y

i +sin

~

i

d

i 1

A

: (13)

Assuming, as for inroom, that theworldis partitionned into P

int and P

ext , one

andene

Test leg in

i

(p): leg in

i

(p)=in room(s

i

(p))_in room(

i ).

Thefollowingresultexplainswhythistestanbeusedinonjuntionwithdat

i to

eliminateongurations.

Proposition1 leg in

i

(p)=0 )dat

i

(p)=0. }

Proof: leg in

i

( p)=0impliesthats

i isinP

int and

i inP

ext

(seeFigure6).Then

there exists j suh that [s

i

;

i

℄\[a

j

;b

j

℄ 6= ;and s

i

2

ajbj

. Let m

ij

=[s

i

;

i

℄\

[a

j

;b

j

℄. Theithoneintersets[a

j

;b

j

℄atleastatm

ij

. Sotheremotenessof[a

j

;b

j

℄

from s

i

is less thanor equalto k

!

s

i m

ij

k. As m

ij 2 [s

i

;

i [; k

!

s

i m

ij k< k

!

s

i

;

i k=

d

i

, and the remoteness of [a

j

;b

j

℄ from s

i

is therefore inompatible with [d

i

℄; so

dat

i

(p)=0.

Thetestlegin

i

(p)thusprovidesaneessaryonditionforptobeonsistentwith

theith measurement. Asthis onditionisnotsuÆient,leg in

i

(p)mayholdtrue

even when dat

i

(p) holds false.It will only be useful to eliminatesome unfeasible

ongurationsmorequikly.

4. Interval tests

Thetestspresentedinthepreedingsetionforpointongurations,shouldnowbe

extendedto intervalongurations. The notionof Boolean intervals will be used

totakethepossibleambiguityoftest resultsinto aount. Itwillthenbepossible

to give interval ounterpartsof the loalization tests, whih will be assoiated to

inreasetheireÆieny.

4.1. Booleanintervals andinlusion tests

A Boolean intervalis anelementof IB = f0;[0;1℄;1g, where 0stands forfalse, 1

for true and [0;1℄ for indeterminate. It is a onvenient objet for implementing

three-valuedlogi.

Table1speiestheAND(^)andOR(_)operationsbetweentwoBooleanintervals.

As Boolean intervalsare sets, standardset operatorssuh as[and \also apply.

They should not be onfused with the logial operators _ and ^. For instane,