Bearing-only SLAM: comparison between probabilistic and deterministic methods

HAL Id: inria-00308722

https://hal.inria.fr/inria-00308722

Submitted on 1 Aug 2008

HAL is a multi-disciplinary open access

archive for the deposit and dissemination of

sci-L’archive ouverte pluridisciplinaire HAL, est

destinée au dépôt et à la diffusion de documents

and deterministic methods

Cyril Joly, Patrick Rives

To cite this version:

Cyril Joly, Patrick Rives. Bearing-only SLAM: comparison between probabilistic and deterministic

methods. [Rapport de recherche] RR-6602, INRIA. 2008, pp.91. �inria-00308722�

a p p o r t

d e r e c h e r c h e

9

-6

3

9

9

IS

R

N

IN

R

IA

/R

R

--6

6

0

2

--F

R

+

E

N

G

Thème NUM

Bearing-only SLAM : comparaison entre une

méthode probabiliste et une méthode déterministe

Cyril Joly — Patrick Rives

N° 6602

CyrilJoly

∗

, Patri k Rives

∗

ThèmeNUMSystèmesnumériques

ProjetARobAS

Rapportdere her he n°6602Juillet200891pages

Résumé : Ce travail de re her he s'ins rit dans la problématique de la lo alisation et

artographiesimultanée(SLAMenanglais).Lesappro hes lassiquespourrésoudreleSLAM

sontbaséessurleltredeKalmanétendu(EKF-SLAM)ouleltreparti ulaire(FastSLAM).

Cesdeuxfamillesd'algorithmespermettentunerésolutionenligneduproblèmemaisposent

desproblèmesd'in onsistan e.Dans erapportnousnenousintéressonspasauxappro hes

pré itéesmaisàdesappro hesglobales.Celles- iné essitentd'utiliserl'ensembledesmesures

a quises pour al uler l'ensemble de la traje toire du robot et la lo alisation des amers.

Bienquelesappro hesglobalesne permettentpasuntraitementenligne, ellenesont pas

dénuées d'intérêt : elles sont a priori moins soumises aux problèmes d'in onsistan e que

l'EKF-SLAM ou leFastSLAM.Nous nous intéressonsà deuxméthodes : le GraphSLAM,

basé sur une appro he probabiliste gaussienne, et le SLAM par intervalles, basé sur une

appro he déterministe. Ces méthodes sont omparées à l'aide de simulations dans le as

du SLAM bearing-only. Nous avons simulé une traje toire ir ulaire plane pour le robot.

Les amers,quant àeux, sontdes points 3Ddont onmesure le gisementet l'élévation par

rapport au robot. Les résultats mettent en éviden e la onsistan e des deux algorithmes

lorsqueleserreurssont entrées.Dans e aspré is,leGraphSLAMdélivredebienmeilleurs

résultatsqueleSLAMparintervallesentermedetailledeszonesde onan e.Enrevan he,

si les données sont biaisées, l'algorithme de GraphSLAM est in onsistant. Le SLAM par

intervalles,quantàlui,délivredesrésultats onsistantstoutàfait ohérents.

Mots- lés : lo alisation, artographie,analysegaussienne,analyseparintervalles

∗

INRIASophia-AntipolisProjetARobAS, 2004Route desLu ioles,BP93,06902 Sophia-Antipolis

Abstra t: This work deals with the problem of simultaneous lo alization and mapping

(SLAM). Classi al methods for solving the SLAM problem are based on the Extended

KalmanFilter(EKF-SLAM)orparti lelter(FastSLAM).Thesekindsofalgorithmsallow

on-line solving but ould be in onsistent. In this report, the above-mentioned algorithms

arenotstudied but globalones.Global approa hesneedall measurementsfrom theinitial

stepto the nal stepin order to ompute thetraje toryof the robot and the lo ation of

the landmarks. Even if global approa hes do not allow on-line solving, they anbe more

interestingthan EKF-SLAM orFastSLAMsin ethey are lesssensitiveto in onsisten ies.

Two algorithms are studied: the GraphSLAM, a probabilisti method based on gaussian

hypothesis, andthe "intervalSLAM" whi h is adeterministi approa h. A omparison of

thealgorithms ismadein simulation,forthe bearing-only ase.A ir ularand planar

tra-je tory for the robot is simulated. Landmarks are 3D points from whi h we measure the

bearingandelevationangleswithrespe ttotherobot.Theresultsshowthe onsisten yof

bothalgorithmswhenthe errorsare entered. Inthis ase,if welook thesize ofthebelief

areasprovidedbythealgorithms,GraphSLAMdeliversbetterresultsthanintervalSLAM.

Finally,theGraphSLAMalgorithmbe omesin onsistentwheninputdataarebiased.Inthe

latter ase,intervalSLAM givesgoodand onsistentresults.

Table des matières

1 Introdu tion 6

2 Hypothèses généralesdu SLAM 7

2.1 Notationsgénériques . . . 7

2.2 Appli ationauSLAM 2D/3Ddansle asd'unrobotnonholonome . . . 9

2.2.1 Cadredel'étude . . . 9

2.2.2 Paramétrisation. . . 9

2.2.3 Equationsd'évolutiondurobot . . . 10

2.2.4 Equationsdemesures . . . 12

3 Présentationdu GraphSLAM 12 3.1 RappelssurleSLAM probabiliste. . . 12

3.1.1 Rappelsgénéraux. . . 12

3.2 Des riptionduGraphSLAM . . . 13

3.2.1 Représentationparmatri ed'informationet ve teurd'information . . 14

3.2.2 Densitéaposteriori delatraje toireetdela arte . . . 15

3.2.3 Logarithmedeladensitéa posteriori . . . 17

3.2.4 ExpansiondeTaylor . . . 17

3.2.5 Rédu tiondelamatri ed'informationetduve teurd'information . . 18

3.2.6 Obtenirlespointsdefon tionnement. . . 21

3.2.7 Con lusion . . . 22

4 Mise en appli ationdu GraphSLAM 22 4.1 Matri esja obiennesdesfon tions

f

et

h

. . . 22

4.1.1 Matri e ja obiennedelafon tiond'évolution . . . 22

4.1.2 Matri e ja obiennedelafon tiondemesuresparrapportà

x

t

. . . . 22

4.1.3 Matri e ja obiennedelafon tiondemesuresparrapportà

m

. . . . 23

4.2 Matri esdevarian es- ovarian esutilisées . . . 23

4.2.1 Matri e devarian es- ovarian esliéeàlamesure . . . 23

4.2.2 Matri e devarian es- ovarian esliéeaumodèle . . . 24

4.3 Initialisation. . . 25

4.3.1 Initialisationdelatraje toire . . . 25

4.3.2 Initialisationdelaposition desamers . . . 26

5 Présentationdu SLAM par intervalles 28 5.1 Redénitiondesopérateurs lassiques . . . 29

5.2 Appli ationauSLAM . . . 30

5.2.1 Traitementlo al:passagede

t

à

t + 1

. . . 30

6 Mise en appli ationdu SLAM par intervalles 34

6.1 Fon tionsutilisées . . . 35

6.1.1 Equationdemodèle . . . 35

6.1.2 Equationdemesure . . . 36

6.2 Initialisationdesamers. . . 37

6.3 Choixdel'orientationdesaxes . . . 38

6.3.1 Remarque générale . . . 38

6.3.2 Exemple 1:modèled'évolutiondurobot . . . 38

6.3.3 Exemple 2:fon tiondemesure . . . 41

6.3.4 Solutionproposée. . . 41

6.3.5 Etude dugainapportésurunesimulation . . . 42

7 Résultatsobtenus 46 7.1 Des riptiondessimulations . . . 46

7.1.1 Cara téristiquesdelasimulation . . . 46

7.1.2 Modeopératoire . . . 46

7.1.3 Résultatsétudiés . . . 47

7.2 Le asgaussien entré . . . 48

7.2.1 GraphSLAM . . . 48

7.2.2 SLAM parintervalles . . . 54

7.2.3 Con lusionquantau asgaussien entré . . . 54

7.3 Casuniforme entré . . . 58

7.3.1 GraphSLAM . . . 58

7.3.2 SLAM parintervalles . . . 61

7.3.3 Con lusionquantau asuniforme entré. . . 61

7.4 Casuniformebiaisé. . . 64

7.4.1 GraphSLAM . . . 65

7.4.2 SLAM parintervalles . . . 70

7.4.3 Con lusionquantau asbiaisé . . . 74

7.5 Résultatsenvisibilité réduite . . . 74

7.6 Remarquesurla onsistan e duGraphSLAM . . . 80

7.7 Tempsde al ul. . . 81

7.7.1 GraphSLAM . . . 81

7.7.2 SLAM parintervalles . . . 81

7.7.3 En pratique . . . 81

7.8 Con lusionquantauxsimulations. . . 81

A Coe ients asso iésaux ellipsesde onan e 86

A.1 Généralités . . . 86

A.2 Casparti uliertraité . . . 86

A.3 Cas2D:ellipsesde onan e . . . 87

A.3.1 Cal ulde

K

. . . 87

A.3.2 Cal uldel'airedel'ellipse . . . 88

A.4 Cas3D:ellipsoïdesde onan e . . . 88

A.4.1 Cal ulde

K

. . . 89

1 Introdu tion

Lesproblèmesdelo alisationetde artographiesontdesproblèmesmajeursenrobotique

mobile.Onparledelo alisationlorsqu'on her heàpositionnerunrobotparrapportàune

arte onnue.Lorsqu'on her heà onstruireunereprésentationdel'environnement

onnais-santlatraje toiredurobot,onparleau ontrairedere onstru tionoude artographie.Ces

deux problèmes sont bien onnus et aujourd'hui résolus.Si on her he àretrouverla

tra-je toiredurobotet àre onstruirel'environnement,onparledelo alisationet artographie

simultanée,ouSLAM(SimultaneousLo alizationandMapping).Ils'agitd'unproblème

en-tralsionveut on evoirdesrobots omplètementsautonomes.Unedesappli ationssouvent

itéeestl'explorationautonomed'environnementsin onnuset/oudangereux([9℄).

Le problèmeduSLAM aétéabordé dans lesannées1980 ave lestravauxdeSmith et

Cheeseman([17℄). Dans ettepremière appro he, les auteurs utilisent un ltre de Kalman

étendu:onparled'EKF-SLAM.L'EKF-SLAMestlongtempsrestél'appro heprin ipalede

résolutionduproblèmedeSLAM.Ellepermetd'évaluerenligne(danslalimitedelavitesse

de al ul de lama hineutilisée) l'étatdu robot ainsique la arted'amer. Cetteappro he

est en ore beau oup utilisée aujourd'hui ([4, 13, 16℄). Néanmoins, elle n'est pas exempte

derepro hes.En eet,plusieurspubli ationsmettentenéviden e le ara tèrein onsistant

de l'EKF-SLAM : les ellipses de onan e déduites des matri es de varian es- ovarian es

sonttroppetitesparrapportàl'erreurréelle([2, 8℄).Deplus, l'EKF-SLAMné essite

l'in-versiondelamatri edevarian es- ovarain esdel'état. 1

Le oûtde ette inversiongrandit

ave le arré du nombre d'amers. Pour orriger e dernier point, Montemerlo a proposé

l'algorithme de FastSLAM ([10, 11, 12℄). Cet algorithme utilise un ltre parti ulaire

Rao-Bla kwellisépourrésoudreleproblèmeduSLAM:unjeudeparti ulespondéréesreprésente

plusieurshypothèses on ernantl'étatdurobot.A haqueparti uleestensuiteasso iéeune

artediérente.L'utilisation dultre parti ulairepermet de rendre l'ensemble des amers

indépendantspourune parti ule donnée(il s'agit de la fa torisationdu SLAM

ondition-nellementàlatraje toire).Ainsi, l'algorithmede FastSLAMpermet degarantiruntemps

d'exé ution beau oup plus faible quel'EKF-SLAM. En revan he, et algorithmen'est pas

onsistant, e ià auseduphénomènededégénéres en edesparti ules ([3℄).Cesméthodes,

ainsique lades ription omplète du problèmeduSLAM sontdé rits dans lestutoriels de

TimBailey([1,6℄).

Nous proposons dans e rapport de re her he d'étudier et de omparer deux autres

algorithmespourrésoudreleproblèmeduSLAM.Ils'agitdedeuxméthodesglobales.

Con-trairementauxappro hesdetypeEKF-SLAMouFastSLAM(quitraitentlesinformations

àl'instant

t

uniquementàl'aidedesmesuresàl'instant

t

etde l'étatàl'instant

t − 1

),les appro hesglobalesrésolventleproblèmeduSLAMsurl'ensembledelatraje toire( e i

pou-vant né essiterplusieursitérationsavant onvergen e).Cela permet en théoried'améliorer

l'estimationdel'état àl'instant

t

àl'aided'informationsa quisesauxinstants

t

′

_{> t}

.Dans

e as,onparleengénéraldeSAM (Smoothing And Mapping).De tellesméthodesnesont

a priori pasadaptées au temps réel ouà l'évaluation en ligne de l'état. Néanmoins, elles

1

ne sont pas dénuéesd'intérêt, notammenten e qui on ernela onsistan e. La première

méthodeétudiée est leGraphSLAM, proposé parThrunet Montemerlo([18℄). Lase onde

méthodeest une méthode originalebaséesur l'analysepar intervalles. Cesdeux méthodes

sontsommairementdé ritesdansleparagraphesuivant:

GraphSLAM : il s'agit d'une méthode de ltrage et de lissage probabiliste basée sur

l'hypothèse gaussienne. L'ensemble des mesures ee tuées est utilisé pour

onstru-ireune estimation del'ensemble dela traje toireet desamersainsi quelesmatri es

devarian es- ovarian esasso iées.Lestempsde al ulrestenta eptablesdepart

l'u-tilisation d'un ltred'information. Lades ription de ette méthode faitl'objet dela

se tion3.

SLAM parintervalles: il s'agit d'uneméthodedéterministepourre onstruirela

tra-je toire du robot ainsi que l'ensemble des amers. L'idée est i i de supposer que les

erreurs sont omprisesdans desintervalles onsistantsan dedéduire desintervalles

surlapositiondurobotetdesamers.Au uneapproximationn'estfaite.Lades ription

généralede etteméthodefaitl'objetdelase tion5.

Les deux algorithmes omparés utilisent des hypothèses diérentes on ernant les

er-reurs. Le GraphSLAM utilise l'idée répandue que les erreurs respe tent une distribution

gaussienne(tout ommel'EKF).Lase ondeméthode,quantàelle,nefaitqu'unehypothèse

debornitude.La omparaisondesperforman esserafaitegrâ eàdessimulations.Lafaçon

de générerleserreurs ne serapas inno ente.Une dis ussion détaillée est ee tuée dansla

se tion7.

Ce do ument s'arti ule en 6 parties. Tout d'abord, nous rappelonsbrièvement les

hy-pothèsesee tuéesdansle adrede ette étude.Lase tion 3présentel'algorithme général

deGraphSLAM. L'appli ationduGraphSLAMànotre asestdé ritedanslase tion4.La

se tion5de edo umentprésentequantàelledesgénéralitéssurlesméthodesderésolution

parintervalles.Nousprésentonsensuitelamiseen÷uvrede etalgorithme(se tion6).Nous

présentonsenndesrésultats omparatifsobtenusensimulation(se tion7).

2 Hypothèses générales du SLAM

Nous présentonsdans ettese tion leshypothèseset notationsemployées danslasuite

de edo ument.Lapartie2.1présentelesnotationsgénériquesutiliséestandisquelapartie

2.2présenteleséquationsutiliséesdansnotre asparti ulier.

2.1 Notations génériques

L'obje tif du SLAM est de retrouver à la fois la traje toire d'un robot et la position

d'amers (supposés statiques dans notre étude).Il s'agit don d'ee tuerla re onstru tion

del'étatdurobotàtouslesinstantsainsi quel'étatdel'ensembledela arted'amers.Les

notationsemployéessontprésentéesdansle tableau1. Onpourranoter quelorsqu'unétat

ouune mesure désigneunamer,l'indi e de l'amerest notéentre parenthèses an d'éviter

Notation Des ription Valeursdans

x

t

Etatdurobotàl'instant

t

R

n

N

Nombred'amers

N

m

(i)

Etatdel'amernuméro

(i)

R

l

(i)

m

Etatdel'ensembledesamers

R

P

N

i=1

l

(i)

u

t

Entréepourl'évolutiondurobot

R

p

z

t,(i)

Mesureasso iéeàl'amer

(i)

etàl'instant

t

R

q

(i)

z

t

Con aténations de toutes les mesures à

l'in-stant

t

R

P

N

i=1

q

(i)

f

Fon tion d'évolutiondu robot (dépend de

x

t

etde

u

t

)

R

n

_{× R}

p

_{→ R}

n

h

_(i)

Fon tion de mesurede l'amer

(i)

(dépend de

x

t

etde

m

(i)

)

R

n

_{× R}

l

(i)

_{→ R}

q

(i)

h

Fon tiondemesures(dépend de

x

t

etde

m

)

R

n

_{× R}

P

N

i=1

l

(i)

_{→ R}

P

N

i=1

q

(i)

ν

XX

t

Erreur asso iée à

XX

à l'instant

t

(

XX

: ve teurgénérique)

R

dim(XX)

XX

t

1

:t

2

Con aténationdesve teurs

XX

t

entreles in-stants

t

1

et

t

2

R

dim(XX)(t

2

−t

1

+1)

Tab.1Notationsutilisées

Remarque2.1

Ilestenvisageabledetraiterdesamersdediérents typesetdon paramétrésdiéremment.

Ce iexplique lanotation

l

(i)

pourla dimensionde l'étatdel'amer

(i)

et

q

(i)

pourla dimen-sionde la mesuredel'amer

(i)

.

Pourla suite, onposera

l =

P

N

i=1

l

(i)

et

q =

P

N

i=1

q

(i)

.

Dans etteétude,onsuppose quel'évolutiondel'état durobotentre deuxinstantsest

donnéeparlafon tion

f

et leve teurd'entrées

u

t

(supposémesuré):

x

t+1

= f (x

t

, u

t

)

(1)

Lorsque la fon tion

f

ne représente pas exa tement la réalité et/ou que le ve teur

u

t

n'estpasexa tementleve teurattendu,onaura:

x

t+1

= f (x

t

, u

t

+ ν

t

u

) + ν

f

t+1

(2)

où

ν

u

t

désignel'erreursurl'entréeet

ν

f

t+1

l'erreurdemodèleàl'instant

t

.

Ensuite, onsupposequeleve teurdemesurespeutsedéduiredel'étatgrâ eàla

fon -tion

h

:

z

t

= h (x

t

, m) + ν

t

z

(3)

où

ν

z

t

désignel'erreursurlesmesuresàl'instant

t

.

mesures

z

t

etdesentrées

u

t

.Lesmesuresetentréesétantenta héesd'erreur,lesalgorithmes deSLAMdevrontêtrerobustesvisàvisd'elles.

2.2 Appli ationauSLAM2D/3Ddansle asd'unrobotnonholonome

Nousprésentonsdans ettepartieleséquationsutiliséesdansle adreduSLAM2D/3D.

NousdésignonsparSLAM2D/3DlaproblématiqueduSLAMappliquéeau asd'unrobotse

déplaçantdansunplanetobservantdesamersrépartisdanstoutl'espa e.Nousdénissons

d'abordle adredel'étude.Nousprésentonsensuitelaparamétrisation hoisie.Enn,nous

exposonsleséquations(demodèleet demesures)utilisées.

2.2.1 Cadre de l'étude

CetteétudetraiteduSLAMdans le as bearingonly : seulslesanglesrelatifsentre

lerobotetlesamerssontmesurés.Typiquement,une amérafournit etyped'information.

Cetteappro heest enoppositionave le as rangeandbearing,dontle apteurtypique

estlelaser.Nousjustionsnotreappro heparlefaitqueles apteurspermettantd'obtenir

l'information de distan e sont oûteux et né essitent plus de temps d'a quisition ( as du

laser3D)oudetraitement( asd'unepairede améras).

Par ailleurs, nous ne traitons i i que le as 2D en e qui on erne le robot : le robot

évolue dans un plan horizontal. L'état du robot est don paramétré par ses oordonnées

dansleplan etsonorientation.Lesamers,quantàeux,sontdespoints3D. 2

Lesméthodes

généralesproposéessontnéanmoinsappli ablesau asoùtoutesten3D(ilfautalorsadapter

lesfon tionsutilisées).

Enn,noussupposonsquelerobotvéreune ontraintedenon-holonomie:sa vitesse

tranversale est nulle. Ce i équivautà supposer que lerobot évoluele longd'un ar de

er leentredeuxinstants onsé utifs.

2.2.2 Paramétrisation

Nousexpli itons dans e paragraphelesve teurs

x

t

,

u

t

et

z

t

utilisés( f.gure1):

x

t

: nous supposons que le robot évolue dans le plan. A l'instant

t

, il est repéré par sa positiondanslerepèreabsolu(

x

t

, y

t

)etsonorientationparrapportàl'axedesabs isses (

θ

t

). L'environnement étantentièrement in onnu, le repère absolu est hoisi égalau repèreinitial durobot.

3 Ona:

x

t

=



x

t

y

t

θ

t



T

(4)

u

t

: lerobotestvu ommeunsolideenmouvement,animéparuntorseur inématiquequi serautilisé ommeve teurde ommande.Etantdonné quenoussommesdans le as

2D,leve teurvitesseà haqueinstant

t

estdansleplanduvéhi ule.Ilestrepérépar 2

Le asd'unrobotévoluantenintérieurillustrebien eshypothèses. 3

sesdeux omposantesexpriméesdanslerepèredurobot(

V

x

t

,

V

y

t

).Leve teurrotation est quantàluiperpendi ulaireauplan duvéhi uleet est paramétréuniquement par

lavitessederotationinstantanée(

ω

t

).Onadon :

u

t

=



V

t

x

V

y

t

ω

t



T

(5)

Une mesurede e ve teurde ommandeest fournie parl'odométrie du robot, qui

nousdonneunevitessedetranslationaxiale(supposéeêtreégaleà

V

x

t

)etunevitesse de rotation(supposée êtreégaleà

ω

t

).Enn,

V

y

t

est xée à 0 pour respe ter la ontrainte de non-holonomie(mesurevirtuelle).

4

m

_(i)

: nous traitons dans ette étude des amers pon tuels pla és dans l'espa e. Ils sont repérésparleurs oordonnées artésiennes(

x

(i)

,

y

(i)

,

z

(i)

).Onadon :

m

(i)

=



x

(i)

y

(i)

z

(i)



T

(6)

z

t

: etteétudetraite du as  bearing-only  : noussupposons que nousn'avonsquedes informations angulaires pour haque amer. Nous supposons par ailleurs qu'il s'agit

d'informationshautniveau(pouvantêtreparexempledéduitedutraitementdel'image

d'une améra):legisement(

α

t,(i)

)etl'élévation(

β

t,(i)

).Onadon :

z

t

=



α

t,(1)

β

t,(1)

. . .

α

t,(N )

β

t,(N )



T

(7)

2.2.3 Equations d'évolution durobot

Lerobotétantvu ommeunsolidedontletorseur inématiqueest

u

t

,ses oordonnées vérient(dansledomaine ontinu)lesystème8:







˙x

= V

x

cos θ − V

y

sin θ

˙y

= V

x

_{sin θ + V}

y

_{cos θ}

˙θ = ω

(8)

Supposonsdésormaisquel'entréedusystèmesoit onstante entre lesinstants

t

et

t + 1

etsoit égaleà

u

t

.L'intégrationdusystème 8nousdonnelafon tion

f

:







x

t+1

=

x

t

+

sin

ω

t

dt

2



V

x

t

dt cos θ

t

+

ω

t

₂

dt

− V

t

y

dt sin θ

t

+

ω

t

₂

dt



y

t+1

=

y

t

+

sin

ω

t

dt

2



V

x

t

dt sin θ

t

+

ω

t

₂

dt

+ V

t

y

dt cos θ

t

+

ω

t

₂

dt



θ

t+1

=

θ

t

+ ω

t

dt

(9)

où

dt

désigneletempsé ouléentrelesinstants

t

et

t + 1

. Deuxsour esd'erreursinterviennentdans lesystème9:

4

Néanmoins,nous entenons omptedans leséquationspourpouvoirtenir omptedu asoùdelégers

glissementslatérauxapparaissent.Ainsi,nous onsidéronsquelamesurevirtuelle

V

y

PSfragrepla ements

−−→

x

rob

−−→

y

rob

−−→

z

rob

α

t,(i)

β

t,(i)

−

→

_x

−

→

_y

−

→

_z

θ

t

m

(i)

Fig.1NotationsduSLAM 2D

1. la ommande

u

t

estmal onnue.Celapeutarriveren asd'impré isionsurl'odométrie. Des asplusextrêmespeuventapparaîtreen asde dérapage: l'équation mé anique

reste vraie mais les torseurs inématiques utilisés pour intégrer l'équation n'ont en

généralplusau unlienave laréalité(leurmesureétantbaséesurl'odométrie).

2. la ommande varie ontinuement entre les instants

t

et

t + 1

. Nous supposons que l'é hantillonnageestsusammentnpournégliger ephénomèneetpouvoirleramener

àunbruit additifde faible amplitude (

ν

f

t+1

dansl'équation2).

En pratique, l'odométrie ne nous permet pas d'a éder aux vitesses. Dans le as du

robotnon-holonomequenousétudions(dontl'équationde ontrainteest

V

y

t

= 0

),nousne

disposonsquedel'in rémentderotation(

ω

t

dt

)etdeladistan epar ourue(

V

x

t

dt

)entreles instants

t

et

t + 1

.Ennotant

ds

x

t

= V

t

x

dt

,

ds

y

t

= V

y

t

dt

et

dω

t

= ω

t

dt

,l'équation9peutêtre é ritedelafaçonsuivante :







x

t+1

= x

t

+

sin

dω

t

2



ds

x

t

cos θ

t

+

dω

₂

t

− ds

y

t

sin θ

t

+

dω

₂

t



y

t+1

= y

t

+

sin

dω

t

2



ds

x

t

sin θ

t

+

dω

₂

t

+ ds

y

t

cos θ

t

+

dω

₂

t



θ

t+1

= θ

t

+ dω

t

(10)

Remarque2.2

Ainsi,l'équationde modèle faitappelàdeuxentréesmesuréesetune entréevirtuelle.

2.2.4 Equations de mesures

Leséquationsdemesures,quantàelles,sontdonnéespardes onsidérationsgéométriques;

lorsque lesmesures sontparfaites, elles- i s'é riventen fon tionde l'état durobot et des

amers(fon tion

h

):







α

t,(i)

=

arctan2

y

(i)

− y

t

, x

(i)

− x

t

− θ

t

β

t,(i)

=

arctan



z

(i)

√

(x

(i)

−x

t

)

2

+((y

(i)

−y

t

)

2



(11)

Remarque2.3 (Valeurs possiblespour lesangles mesurés)

Lesvaleursadmissiblespour

(α

t,(i)

, β

t,(i)

)

sontprisesdans

[−π, π]×[−π/2, π/2]

.Ce ipermet de garantirune bije tion entrel'ensemble des ouples angulaires possibleset l'ensembledes

ve teursà3 omposantesetde norme (eu lidienne)unitaire.

C'est pourquoi la dénition de

α

t,(i)

utilise la fon tion

arctan2

alors que la dénition de

β

t,(i)

n'utilise que

arctan

.

3 Présentation du GraphSLAM

Nousprésentonsdans ettese tionl'algorithmedeGraphSLAM.Ils'agitd'uneméthode

probabiliste basée sur l'hypothèse gaussienne. Contrairement àl'EKF-SLAM ou au

Fast-SLAM,l'estimationdel'étatdurobotsefaitenutilisantl'ensembledesinformationsdepuis

l'instantinitial.Nousverronsque e iestpossibledansuntempsraisonnablegrâ eà

l'util-isationdelamatri ed'informationdeladensitédeprobabilité.Cettese tionest diviséeen

deuxparagraphes.Nousee tonsdansunpremiertempsdesrappelsgénérauxsurleSLAM

probabiliste.Ledeuxièmeparagraphedé ritendétails l'algorithme deGraphSLAM.Cette

des riptionest essentiellementtiréede[18℄.

Lesparagraphessuivantsdé riventendétaillesdéveloppementsmathématiquesqui

per-mettentla onstru tiondelamatri ed'informationetlarésolutionduproblèmed'inféren e

nal.

3.1 Rappels sur le SLAM probabiliste

3.1.1 Rappelsgénéraux

Dans lalittérature,leproblèmeduSLAMest engénéralrésoludemanièreprobabiliste

etestposédefaçonré ursive.L'obje tifestalorsdedéterminerà haqueinstant

t

ladensité deprobabilité de

x

t

(ou

x

0:t

)etdela artesa hantl'ensembledesmesures,des ontrleset l'étatinitial.Ils'agitdon detrouverpourtout

t

:

Dansle asdel'EKF-SLAM,on her heà al ulerà haqueinstant

t

ladensitédeprobabilité

p (x

t

, m|z

0:t

, u

0:t

, x

0

)

. Dans le as présenté i i, àsavoirle GraphSLAM, nous her hons à al uler

p (x

0:t

, m|z

0:t

, u

0:t

, x

0

)

.

Deplus,nousee tuonsles3hypothèsessuivantes :

1. L'ensemble desétatsdurobot formeune haînedeMarkov.Cela signiequesi

x

t−1

et

u

t

sont onnus, la onnaissan edesétatspré édents, des ommandespré édentes et desamersestinutile.Ce isetraduitpar:

p (x

t

|x

t−1

, u

t

) = p (x

t

|x

t−1

, u

t

, x

0:t−2

, u

0:t−1

, m)

(13) 2. Les mesures sont onditionnellement indépendantes (au ours du temps). Ce i se

traduitpar:

p (z

0:t

|x

0:t

, m) =

t

Y

i=0

p (z

i

|x

0:t

, m)

(14)

3. Lamesureàl'instant

i

nedépend quedel'étatdurobotàl'intant

i

etdelaposition desamers.Onaen onséquen e:

p (z

i

|x

0:t

, m, . . .) = p (z

i

|x

i

, m)

(15) Ces3hypothèsessontréalistes.Eneet,lapremièresupposequel'onpeutobtenirlaposition

duvéhi uleenneseservantquedeladernièrepositionetdu ontrleappliquédepuis ette

position (utilisation de l'équation de modèle 10). La deuxième hypothèse, quant à elle,

indiquequelesbruitsdes apteursnesontpas orrélésdansletemps, equi estégalement

assezréaliste. Enn,les apteursque l'on utilisemesurentla position relativedu robot et

desamersàuninstantbien pré is, equi justieladernièrehypothèse.

Unedernièrehypothèseestutilisée:ils'agitdel'hypothèsegaussienne.Cettedernière

est utilisée dans la plupart des appro hes probabilistes (hypothèse de base du ltre de

Kalman étendu utilisé dans l'EKF-SLAM et dans la partie EKF du FastSLAM). Cette

hypothèse onstitueégalementunedesbriquesdebasedesdéveloppementsmathématiques

duGraphSLAM.

3.2 Des ription du GraphSLAM

L'algorithmedeGraphSLAM tireprotdu ara tère éparsdelamatri e d'information

asso iéeàl'état(traje toiredurobotet amers).Ce iprovientdel'interprétationgraphique

duproblèmeduSLAM,etnotammentdufaitque onditionnellementàtoutelatraje toire,

lesamerssontindépendantsentre eux.Le prin ipe duGraphSLAM est de dénirunétat

qui ontient touteslespositions depuisl'instantinitial ainsi quelaposition desamers.La

matri ed'information(voirse tion3.2.1pouravoirdesrappelssur ettereprésentation)du

système est onstruitede manièrein rémentale ( f. gure2). Nousverronsque lamatri e

d'informationetleve teurd'informationnenousdonnentpasdire tementlatraje toiredu

Fig.2Constru tionin rémentaledelamatri ed'informationtotale ([18℄)

in rémentalementlamatri e d'information; e i apoureet d'ajouterdes termes dansla

matri ed'information( f.gure3).

Lesparagraphessuivantsdé riventendétaillesdéveloppementsmathématiquesqui

per-mettentla onstru tiondelamatri ed'informationetlarésolutionduproblèmed'inféren e

nal.

3.2.1 Représentation parmatri e d'informationet ve teur d'information

Danslespro hainesparties,nousparleronsdelareprésentationdedensitédeprobabilité

gaussiennepar ve teurd'informationet matri e d'information.Nousproposonsdans ette

partie de présenter de brefs rappels en e qui on erne ette matri e et e ve teur, et la

relationquiexisteentre les moments lassiques pourreprésenterunegaussienne.

Soient

µ

et

Σ

l'espéran emathématiqueetlamatri edevarian es- ovarian esasso iées àune densitédeprobabilitégaussienne

p (x)

. Onaalors:

p (x) =

p

1

(2π)

n

det Σ

exp



−

1

₂

(x − µ)

T

Σ

−1

_{(x − µ)}



Le prin ipedelareprésentationparve teurd'informationet matri ed'informationest

de trouverun ve teur

ξ

et une matri e

Ω

pour que le fa teur exponentiel de

p

soit dela forme

exp −

1

2

x

T

_Ωx

_{+ x}

T

_ξ

Fig. 3  Marginalisation des amers pour retrouver la pose uniquement. Cette opération

entraînel'ajoutdeliensentrelespositions ([18℄)

Pour efaire,ilsutderé-é rireladensité

p (x)

.Tous al ulsfaits,onobtient:

p (x) =

p

1

(2π)

n

det Σ

exp



−

1

₂

µ

T

Σ

−1

µ



exp



−

1

₂

x

T

Σ

−1

x

+ x

T

Σ

−1

µ



Ainsi,enposant

Ω

= Σ

−1

et

ξ = Σ

−1

_µ

,onobtient:

p (x) =

p

1

(2π)

n

det Ω

−1

exp



−

1

₂

ξ

T

Ω

−1

ξ



exp



−

1

₂

x

T

Ωx

+ x

T

ξ



Ω

estappeléematri ed'informationet

ξ

estappeléve teurd'information.Ilesttrèsfa ile d'identier esparamètreslorsqu'onaune expression quadratique dansune exponentielle.

Lesparamètres lassiques devarian eetd'espéran eseretrouventpar:



Σ

= Ω

−1

µ = Σξ

3.2.2 Densité a posteriori de latraje toire etde la arte

Dans equisuit,on her heàexprimerladensitédeprobabilitéaposteriori de

l'ensem-ble de la traje toire ainsi que de la arte (en supposant les asso iations onnues), soit :

En appliquantlarègledeBayessur

z

t

,onobtient:

p (x

0:t

, m|z

1:t

, u

1:t

) =

p (z

t

|x

0:t

, m, z

1:t−1

, u

1:t

) p (x

0:t

, m|z

1:t−1

, u

1:t

)

p (z

t

|z

1:t−1

, u

1:t

)

(16) soit:

p (x

0:t

, m|z

1:t

, u

1:t

) = η p (z

t

|x

0:t

, m, z

1:t−1

, u

1:t

) p (x

0:t

, m|z

1:t−1

, u

1:t

)

(17) où

η

estune onstantedenormalisation.En appliquantl'équation15surlepremierfa teur del'équation17,onobtient:

p (x

0:t

, m|z

1:t

, u

1:t

) = η p (z

t

|x

t

, m) p (x

0:t

, m|z

1:t−1

, u

1:t

)

(18) En appliquantlarègledeBayes,lese ondfa teur dansl'équation18peuts'é rire:

p (x

0:t

, m|z

1:t−1

, u

1:t

) = p (x

t

|x

0:t−1

, m, z

1:t−1

, u

1:t

) p (x

0:t−1

, m|z

1:t−1

, u

1:t

)

(19) En utilisantl'équation 13,l'équation19devient:

p (x

0:t

, m|z

1:t−1

, u

1:t

) = p (x

t

|x

t−1

, u

t

) p (x

0:t−1

, m|z

1:t−1

, u

1:t

)

(20) Onpeutdésormaisinsérerlerésultatdel'équation20dansl'équation18pourobtenirla

formulationré ursivesuivante:

p (x

0:t

, m|z

1:t

, u

1:t

) = η p (z

t

|x

t

, m) p (x

t

|x

t−1

, u

t

) p (x

0:t−1

, m|z

1:t−1

, u

1:t−1

)

(21) Onpeutalorsmontrerparré urren eque:

p (x

0:t

, m|z

1:t

, u

1:t

) = η

′

p (x

0

, m)

t

Y

k=1

p (x

k

|x

k−1

, u

k

) p (z

k

, |x

k

, m)

(22) où

η

′

estunfa teur onstant(aprioridiérentde

η

).Parailleurs,

p (x

0

, m)

désigneladensité deprobabilitéapriori delapositiondurobotet dela arte.

A haqueinstant

t

,leve teur

z

t

ontientla on aténationdesobservationsee tuéessur plusieursamers.Pourrappel,nousnotons

z

t,(i)

l'observationdel'amer

(i)

àl'instant

t

.Soit

M

t

l'ensembledesamersobservésàl'instant

t

.Poursimplierlasuitedesdéveloppements, noussupposonsqu'àuninstantdonné,lesobservationsde haqueamersontindépendantes

entre elles. Ce i signie don que les ve teurs aléatoires

z

t,(i)

, i ∈ M

t

sont indépendants entre eux. En onséquen e,ladensité aposteriori delatraje toiredu robotet de la arte

s'é rit:

p (x

0:t

, m|z

1:t

, u

1:t

) = η

′

p (x

0

, m)

t

Y

k=1

"

p (x

k

|x

k−1

, u

k

)

Y

i∈M

t

p z

k,(i)

, |x

k

, m

(i)

#

(23)

Remarque3.1 (Eliminationde

m

dans

p (x

0

, m)

)

En général, la densité a priori de la arte et de la position initiale sont indépendantes.

p (x

0

, m)

peutdon s'é rire

p (x

0

) p (m)

.Si la arte est omplètement in onnue,nous pou-vonsintégrer

p (m)

dansla onstanteenfa teur.Dans equisuit, noussupposonsque 'est

3.2.3 Logarithme de la densité a posteriori

Nous allonsdésormais al ulerle logarithme de ladensité al ulée dans l'équation 23.

Entenant omptedelaremarque3.1,nousobtenons:

log p (x

0:t

, m|z

1:t

, u

1:t

) =

onst.

+log p (x

0

)+

t

X

k=1

"

log p (x

k

|x

k−1

, u

k

)

X

i∈M

k

log p z

k,(i)

, |x

k

, m

(i)

#

(24)

Ensupposantquelesbruitsappliquéssurlemodèlesd'évolutionetdemesures(fon tions

f

et

h

),sontadditifs, entrésetgaussiensdematri esdevarian es- ovarian es

Q

t

et

R

t

,on obtient:







p (x

t

|x

t−1

, u

t

) ∝ exp



−

1

2

(x

t

− f

t

(x

t−1

, u

t

))

T

Q

−1

_t

(x

t

− f

t

(x

t−1

, u

t

))



p z

t,(i)

, |x

t

, m

(i)

∝ exp



−

1

2

z

t,(i)

− h(x

t

, m

(i)

)

T

R

−1

_t

z

_t,(i)

_{− h(x}

t

, m

(i)

)



(25)

Enn, en supposant que la densité de probabilité a priori sur la position initiale est

gaussienne, entrée,dematri ed'information

Ω

0

,

p (x

0

)

estdon proportionnelleà

exp x

T

0

Ω

0

x

0

. Onadon aunal:

log p (x

0:t

, m|z

1:t

, u

1:t

)

=

onst.

−

1

2



x

T

₀

Ω

0

x

0

+

P

t

k=1

(x

k

− f

k

(x

k−1

, u

k

))

T

Q

−1

t

(x

k

− f

k

(x

k−1

, u

k

))

+

P

t

k=1

P

i∈M

k

z

k,(i)

− h(x

k

, z

k,(i)

)

T

R

−1

_t

z

_k,(i)

_{− h(x}

k

, z

k,(i)

)



(26)

3.2.4 Expansion de Taylor

L'équation 26 nous donne l'expression du logarithme de la densité a posteriori de la

traje toiredurobot etdela arte.Malheureusement, elle- in'estpasquadratiqueen

x

et

m

, maisseulementen

f

t

et

h

. Nousallonsdésormaislinéariser esfon tions and'obtenir uneexpressionquadratiqueen

x

et

m

.

Soit

µ

x

0:t

unetraje toiredelinéarisationsupposéepro hedelatraje toireréelleet

µ

m

un

pointdelinéarisationpourla arte.Noussupposons

µ

x

0:t

et

µ

m

disponibles;nousdis uterons

delamanièredelesobtenirdansleparagraphe3.2.6.

En supposantlalinéarisationvalide,leséquationsde prédi tionet demesures'é rivent

alors:

(

f

t

(x

t−1

, u

t

) ≈ f

t

(µ

_t−1

x

, u

t

) + G

t

x

t−1

− µ

x

t−1

h(x

t

, m

(i)

) ≈ h(µ

t

, µ

(i)

) + H

x

t,(i)

(x

t

− µ

x

t

) + H

m

t,(i)



m

(i)

− µ

m

(i)



(27)

G

t

désignelamatri eja obiennede

f

t

parrapportà

x

t−1

évaluéen

µ

t−1

;

H

x

t,(i)

désignelamatri eja obiennede

h

parrapportà

x

t

évaluéen

µ

x

t

et

µ

m

(i)

;

H

m

_t,(i)

désignelamatri eja obiennede

h

parrapportà

m

évaluéen

µ

x

t

et

µ

m

(i)

.

Tous al uls faits,l'équation26peutseré-é riredelafaçonsuivante:

log p (x

0:t

, m|z

1:t

, u

1:t

) =

onst.'

−

1

2

x

T

0

Ω

0

x

0

|

{z

}

0

+

P

t

k=1



























−

1

2



x

k

x

k−1



T



I

−G

T

k



Q

−1

k



I

−G

k



|

{z

}

(1)



x

k

x

k−1



+



x

k

x

k−1



T



I

−G

T

k



Q

−

_k

1

(f

k

(µ

k−1

x

, u

k

) + G

k

µ

x

k−1

)

|

{z

}

(2)

+

P

_i∈M

k



























−

1

2



x

k

m

(i)



T

"

H

x

k,(i)

T

H

m

_k,(i)

T

#

R

−1

_k



H

x

_k,(i)

H

m

_k,(i)



|

{z

}

(3)



x

k

m

(i)



+



x

k

m

(i)



T

"

H

x

_k,(i)

T

H

m

_k,(i)

T

#

R

−

_k

1



z

k,(i)

− h µ

x

k

, µ

m

(i)

+



H

x

k,(i)

H

m

k,(i)





µ

x

k

µ

m

(i)



|

{z

}

(4)





















































(28)

Dansl'équation28,nousn'avonsretenuquelestermeslinéairesetquadratiquesen

x

et

m

.Lerestedestermessontdestermes onstants.L'équation28montrequ'ilestainsipossible d'é rire ladensitéaposteriori de latraje toireet dela arteàl'aided'unereprésentation

parmatri ed'informationetve teurd'information.Onaalors:

log p (x

0:t

, m|z

1:t

, u

1:t

) =

onst.'

−

1

2



x

0:t

m



T

Ω



x

0:t

m



+



x

0:t

m



T

ξ

(29)

La matri e

Ω

de l'équation 29 est onstruite à partirdu terme (0) et des termes de type (1)et (3)del'équation 28.Leve teurd'information

ξ

,quantàlui,est onstruitgrâ eaux termesdetype(2)et(4)del'équation28.

3.2.5 Rédu tion de la matri e d'informationetdu ve teurd'information

et d'espéran emathématique.La matri edevarian es- ovarian estotale seraitpleine. Les

auteursde[18℄proposentderéduire etteformeendeuxfa teurs(l'unpourlatraje toirea

posteriori et l'autrepourla arteaposteriori mais onditionnellementàlatraje toire).La

justi ationde etterédu tionestl'équationdefa torisationdeladensitéaposteriori :

p (x

0:t

, m|z

1:t

, u

1:t

) = p (x

0:t

|z

1:t

, u

1:t

) p (m|x

0:t

, z

1:t

, u

1:t

)

(30) Dans eparagraphe,nousmontronsdon ommentobtenirlesparamètres.Danslasuite,

nousallonsutiliserladé omposition anoniquedel'étatentrelatraje toireetla arte.Ainsi,

nouspouvonsadopterlesnotationsintuitivessuivantes:

Ω

=



Ω

x

0:t

,x

0:t

Ω

x

0:t

,m

Ω

m,x

0:t

Ω

m,m



(31)

ξ

=



ξ

x

0:t

ξ

m



(32) 3.2.5.1 Obtentionde

p (x

0:t

|z

1:t

, u

1:t

)

:

Etant donné que les densités de probabilité onsidéréessont gaussiennes, l'estimation

desparamètresde

p (x

0:t

|z

1:t

, u

1:t

)

estdonnéeparlethéorèmedemarginalisation.Ainsi,la matri ed'informationet leve teurd'informationasso iés(notés

Ω

˜

et

ξ

˜

)valent:

˜

Ω

=

Ω

x

0:t

,x

0:t

− Ω

x

0:t

,m

Ω

−1

m,m

Ω

m,x

0:t

(33)

˜

ξ

=

ξ

x

0:t

− Ω

x

0:t

,m

Ω

−1

m,m

ξ

m

(34) L'équation28nousmontrequelamatri e

Ω

m,m

estdiagonaleparblo s:

Ω

m,m

=

diag

Ω

(1),(1)

, . . . , Ω

(j),(j)

, . . . , Ω

(N ),(N )

(35)

où

N

représentelenombretotald'amerset :

Ω

(j),(j)

=

X

k∈τ (j)



H

m

_k,(i)



T

R

k



H

m

_k,(i)



(36)

où

τ (j)

désignel'ensembledesinstantsoùl'amer

j

aétéobservé.Deplus,lamatri e

Ω

x

0:t

,m

s'é rit:

Ω

x

0:t

,m

=



Ω

x

0:t

,(1)

· · ·

Ω

x

0:t

,(j)

· · ·

Ω

x

0:t

,(N )



(37)

ave :

Ω

x

0:t

,(j)

=

















Ω

x

1

,(j)

. . .

Ω

x

k

,(j)

. . .

Ω

x

t

,(j)

















et

(

Ω

x

k

,(j)

=



H

x

k,(i)



T

R

−1

_k



H

m

k,(i)



si

k ∈ τ(j)

Ω

x

k

,(j)

= 0

sinon (38) Au nal,ona:

˜

Ω

= Ω

x

0:t

,x

0:t

−

P

N

j=1

Ω

x

0:t

,(j)

Ω

(j),(j)

−1

_Ω

(j),x

0:t

˜

ξ

= ξ

x

0:t

−

P

j=1

Ω

x

0:t

,(j)

Ω

(j),(j)

−1

_ξ

(j)

(39) ave :

ξ

(j)

=

X

k∈τ (j)



H

m

_k,(j)



T

R

−1

_k



z

k,(i)

− h



µ

x

k

, µ

m

(j)



+

h

H

x

_k,(j)

H

m

_k,(j)

i 

µ

x

k

µ

m

(j)



(40) 3.2.5.2 Obtentionde

p (m|x

0:t

, z

1:t

, u

1:t

)

:

Les paramètres de

p (m|x

0:t

, z

1:t

, u

1:t

)

, quant à eux, s'obtiennent grâ e au théorème sur le onditionnementpar une variable aléatoiregaussienne. En notant

Ω

˜

m

|x

et

ξ

˜

m

|x

les

paramètres her hés,lethéorèmede onditionnementnousdonne:

˜

Ω

m

|x

_{= Ω}

m,m

˜

ξ

m

_|x

_{= ξ}

m

+ Ω

m,x

0:t

x

0:t

(41)

3.2.5.3 Re onstru tionde la traje toire etde la arte

Onpeutdésormaisee tueruneestimationdelatraje toireaposteriorientransformant

lesparamètresd'informationenespéran eet matri e devarian es- ovarian es.Soit

ˆ

x

0:t

et

Σ

x

lesparamètres her hés,ona:

Σ

x

=

Ω

˜

−1

ˆ

x

0:t

=

Σ

ˆ

x

ξ

˜

(42)

L'estimation de la arte et de sa matri e de varian es- ovarian es s'ee tue de façon

similaire:

ˆ

Σ

m

₌

_Ω

−1

m,m

ˆ

m

=

Σ

m

_(ξ

m

+ Ω

m,x

0:t

x

ˆ

0:t

)

(43)

Sa hantquelamatri e

Ω

m,m

estdiagonaleparblo (unblo pour haqueamer),ilenrésulte unedé orrélationdesamers onditionnellementàlatraje toire.Onpeutainsi al ulerune

matri edevarian es- ovarian eset uneespéran epour haqueamer

(j)

:

Σ

_(j)

= Ω

−1

_(j),(j)

ˆ

m

(j)

= Σ

(j)

ξ

(j)

+ Ω

(j),x

0:t

x

ˆ

0:t

=

Σ

(j)

ξ

(j)

+ Ω

(j),τ (j)

x

ˆ

τ (j)

(44)

Lesauteursde[18℄nesuggèrentquede al ulerlesparamètresdela arte

onditionnelle-mentàlatraje toire.Le al uldesvéritablesparamètresaposteriori pour haqueamerest

jugétroplourdétantdonnéqu'iln'yapasdefa torisationpossible omme 'estle as

on-ditionnellementàlatraje toire.Néanmoins,nousee tueronsle al ul ompletdesmatri es

devarian es- ovarian esand'avoiruneestimationdel'in ertitudeasso iéeauxamers.

3.2.6 Obtenirles pointsde fon tionnement

Les développementspré édentssontbasés surle fait qu'il existe undéveloppement de

Taylorpermettant de rendre linéaireles équations de modèle et de mesure. Pour ela, le

pointdelinéarisationdoitêtre susammentpro he delavraie solution.

3.2.6.1 Premièreitération :

Dans un premier temps, nous pouvons initialiser la traje toire en appliquant

simple-ment lafon tion de modèleave les entrées a quises. Ensuite, nous pouvons utiliser ette

traje toireet lesmesurespouree tuer unpla ementapproximatifdesamers(dansle as

bearing-only,onpeutparexempleee tueruneinitialisationauxmoindres arrésdesamers

étantdonnéquel'ondisposedetoutelatraje toire 5

).

Il devient alors possible d'appliquer l'algorithme de GraphSLAM grâ e aux points de

fon tionnementtrouvéspourlatraje toireet la arte.

Remarque3.2

Au unein ertituden'estasso iéeà espointsdefon tionnement.Ilsnedoiventpasêtre

on-sidérés ommeuneinformationaprioriparexemple.Ils'agitjustedesepla ersusamment

prèsde la solutionpour quel'algorithmede GraphSLAM soitvalide.

3.2.6.2 Itérationssuivantes :

Ilestpossiblequelespointsdefon tionnementutilisésàl'originenesoientpasparfaits

(notammentdansle asdemesuresfortementbruitées).Ainsi, lerésultatduGraphSLAM

est peutêtre légèrementbiaisé, tout en étantplus juste quel'initialisation initiale. On va

alorsrelan erl'algorithmedeGraphSLAMenutilisantlespré édentsrésultats ommepoints

defon tionnement.

Cettestratégieitérativeestalorsmaintenuejusqu'à onvergen edurésultat.

5

3.2.7 Con lusion

Lesétapesprésentéesmettentenéviden elaphilosophieduGraphSLAM,àsavoir

l'util-isationdelatraje toiretotaledurobotparlebiaisd'unltred'informationquifaitressortir

lastru turegraphiqueduSLAM.Ilexisted'autresalgorithmesbaséssur eprin ipe, omme

lesquarerootSAM([5℄).Dans e dernier as,desoptimisations sontee tuéesan de

fa -toriserlamatri ed'informationet deréduirelestempsde al ul.

4 Mise en appli ation du GraphSLAM

Nous présentonsdans ettese tionl'appli ationduGraphSLAMau as 2D/3Détudié.

Nous donnons dans un premier temps le détail des matri es ja obiennes utilisées. Nous

expli itons ensuitelesmatri esde varian es- ovarian esutilisées.Enn, unedis ussion est

faite on ernantl'initialisationdelatraje toiredurobotet desamers.

4.1 Matri es ja obiennes des fon tions

f

et

h

L'appli ationdel'algorithmedeGraphSLAMné essitedelinéariserlesfon tions

f

et

h

autourdepointsdefon tionnementsupposéspro hesdelasolutionréelle.Cettelinéarisation

impliquele al ul desmatri es ja obiennes

f

et

h

. Cesmatri espeuventdépendreàlafois del'étatetdesentrées.

4.1.1 Matri e ja obiennede lafon tion d'évolution

Cette matri e représente lesvariations de

x

t+1

par rapport àde petitesvariationssur

x

t

.Elleest notée

F

t+1

etvaut:

F

t+1

=





1 0

−ds

x

t

sin

dω

t

2

sin θ

t

+

dω

₂

t

0 1

ds

x

t

sin

dω

t

2

cos θ

t

+

dω

₂

t

0 0

1





(45) Remarque4.1 (Absen e de

ds

y

t

) Leterme

ds

y

t

n'apparaît pasdansl'équation45 aril aétépréalablement rempla éparzéro.

4.1.2 Matri e ja obiennede lafon tion de mesurespar rapport à

x

t

Cette matri ereprésente les variationsde

z

t

parrapport àdepetites variationsde

x

t

. Etant donnéque l'onmesure haqueamerindépendammentdesautres, on donnera

seule-mentlamatri eja obiennedelafon tiondemesuredel'amer

(i)

,notée

H

x

t,(i)

:

H

x

_t,(i)

=

"

_y

_(i)

_−y

_t

(x

(i)

−x

t

)

2

+(y

(i)

−y

t

)

2

−(x

(i)

−x

t

)

(x

(i)

−x

t

)

2

+(y

(i)

−y

t

)

2

−1

H

x

t,(i)

(2, 1)

H

x

t,(i)

(2, 2)

0

#

(46)

ave :











H

x

t,(i)

(2, 1) =

z

(i)

(x

(i)

−x

t

)

√

(x

(i)

−x

t

)

2

+(y

(i)

−y

t

)

2



(x

(i)

−x

t

)

2

+(y

(i)

−y

t

)

2

+z

(i)

2



H

x

t,(i)

(2, 2) =

z

(i)

(y

(i)

−y

t

)

√

(x

(i)

−x

t

)

2

+(y

(i)

−y

t

)

2



(x

(i)

−x

t

)

2

+(y

(i)

−y

t

)

2

+z

(i)

2



(47)

4.1.3 Matri e ja obiennede lafon tion de mesurespar rapport à

m

De même, les variations des mesures par rapport à de petites variations de la arte

d'amerssontdonnéespar

H

m

t,(i)

:

H

m

_t,(i)

=





−(y

(i)

−y

t

)

(x

(i)

−x

t

)

2

+(y

(i)

−y

t

)

2

x

(i)

−x

t

(x

(i)

−x

t

)

2

+(y

(i)

−y

t

)

2

0

H

m

t,(i)

(2, 1)

H

m

t,(i)

(2, 2)

√

(x

(i)

−x

t

)

2

+(y

(i)

−y

t

)

2

(x

(i)

−x

t

)

2

+(y

(i)

−y

t

)

2

+z

2

(i)





(48) ave :

(

H

m

_t,(i)

(2, 1) =

−H

x

t,(i)

(2, 1)

H

m

_t,(i)

(2, 2) =

−H

x

t,(i)

(2, 2)

(49)

4.2 Matri es de varian es- ovarian es utilisées

Dansle adreduGraphSLAM,toutesleserreurssontsupposéesêtregaussiennes.Nous

dé rivonsi ilesmatri esdevarian es- ovarian esutilisées.Ellessontaunombrededeux :

1.

Q

t

est la matri e de varian es- ovarian es liée à l'erreur ee tuée en appliquant la fon tion

f

.

2.

R

t

est lamatri e de varian es- ovarian esliéeà l'erreuree tuée sur la mesuredes amers.Lesmesuresdesamerssontsupposéesindépendantes.

R

t

s'é ritdon :

R

t

=

diag

R

t,(1)

, . . . , R

t,(N )

4.2.1 Matri e de varian es- ovarian es liéeà la mesure

Lamatri edevarian es- ovarian esliéeàlamesureestdire tementliéeàl'erreurdu

ap-teur.Noussupposons pouvoir ara tériser etteerreur(savarian e)etqu'elleestidentique

pourl'ensembledesamersobservés.Onpose:

∀i ∈ [1 . . . N] R

t,(i)

=

 σ

2

α

0

0

σ

2

β



(50)

Remarque4.2 (Matri e

R

t,(i)

diagonale)

Dans e qui suit, la matri e de varian es- ovarian es de

R

t,(i)

est prise diagonale. Ce i signie que dans les simulations de la se tion 7, les erreurs sur les angles de gisement et

Néanmoins, e i n'est pasné essairement vrai pour les as réels. Par exemple, dans le as

d'uneimageomnidire tionnelle,uneerreursurlalo alisationd'unpointdansl'image

( oor-donnéesenpixels)peutintroduiredes orrélationsentrelesanglesde gisementetd'élévation

(à ausede la géométriedu apteur).

4.2.2 Matri e de varian es- ovarian es liéeau modèle

Lamatri edevarian es- ovarian esdueàl'utilisationdelafon tion

f

pourobtenirune prédi tiondel'étatdurobotàl'instant

t + 1

(équation2)estmoinstriviale.Ilestné essaire deprendreen omptedeuxtypesd'erreurs:

Erreur

ν

f

t+1

: 'est l'erreursur le modèle, agissantde manière additive. Nous supposons onnaître sa matri e de varian es- ovarian eset qu'elle est indépendante du temps.

Nouslanotons

Q

f

.

Erreur

ν

u

t+1

: 'est l'erreur ommise sur la mesure des entrées.

ds

x

t

et

dw

t

sont obtenus par odométrie et enta hés d'erreur. Par ailleurs, nous supposons qu'une vitesse

V

y

( petite )parasite(et nonmesurée) peutégalementêtreprésenteentre lesinstants

t

et

t + 1

sous forme d'un bruit gaussien. Nous notons

Q

u

lamatri e de

varian es- ovarian esasso iée.Noussupposonsque ette matri eestdiagonale(ie.les3erreurs

duesàla ommandesontindépendantes)et onstante: 6

Q

u

=

diag

σ

2

ds

x

, σ

2

_ds

y

, σ

2

_dω

ave

σ

2

ds

y

≪ σ

_ds

2

x

Dansl'algorithmegénéraldeGraphSLAM,ilestsupposéquel'erreursurlemodèleintervient

defaçonlinéaire.Ce iestee tivementle aspour

ν

f

t+1

,maisçanel'estpaspour

ν

u

t

.Une

linéarisationestee tuée.Onaaunal:

Q

t+1

= J

u

t

Q

u

_J

u

t

T

+ Q

f

(51) où

J

u

t

estlamatri eja obienne

f

parrapportà

u

(évaluée en(

x

t

, u

t

)):

J

u

_t

=





sin

dω

t

2

cos θ

t

+

dω

₂

t

−

sin

dω

t

2

sin θ

t

+

dω

₂

t

J

u

t

(1, 3)

sin

dω

t

2

sin θ

t

+

dω

₂

t

sin

dω

t

2

cos θ

t

+

dω

₂

t

J

u

t

(2, 3)

0

0

1





(52) ave :

(

J

u

_t

(1, 3) =

2ds

x

t

dω

2

t

cos θ

t

+

dω

t

2

·

dω

t

2

cos

dω

t

2

− sin

dω

t

2

−

ds

x

t

2

sin

dω

t

2

· sin θ

t

+

dω

₂

J

u

_t

(2, 3) =

2ds

x

t

dω

2

t

sin θ

t

+

dω

t

2

·

dω

t

2

cos

dω

t

2

− sin

dω

t

2

+

ds

x

t

2

sin

dω

t

2

· cos θ

t

+

dω

₂

(53) 6

C'estunehypothèse poursimplierlessimulations.Ellen'estpasné essairedans le asréel(oùilya

Remarque4.3 (sur

Q

t

(3, 3)

)

Leterme

Q

t

(3, 3)

est indépendant du temps( arla matri e

Q

f

est supposée indépendante

du temps et que le terme

(3, 3)

de

J

u

t

Q

u

J

u

t

T

l'est aussi). Par ailleurs, l'erreur du modèle on ernant l'orientation du robot est uniquement due à l'erreur sur la rotation mesurée.

Cetteerreur estprise en omptedans lapartie

J

u

t

Q

u

J

u

t

T

.Onprendradon

Q

f

_{(3, 3) = 0}

.

4.3 Initialisation

L'algorithmede GraphSLAM né essiteenn d'ee tuerune initialisation.Celle- i doit

permettred'obtenirunpremierpoint defon tionnement orre tpourpermettrela

linéari-sationdeséquationsdemodèleet demesures.Ondistinguei il'initialisationdesamersde

elledelatraje toiredurobot.

4.3.1 Initialisation de la traje toire

L'initialisationdelatraje toirepeutsefairegrâ eaumodèle.L'intégrationdel'odométrie

(appli ation de l'équation 10) permet d'obtenir une traje toire susamment pré ise, au

moinssurle ourtterme.Néanmoins,nousn'initialisonspasl'ensembledelatraje toireen

une seule fois (la varian e de l'erreurn'étantpas bornée dans le temps). Plusieurs

publi- ations font état du ara tère ritiquede l'angle de ap durobot lors de la résolution du

problèmedeSLAM([2,7℄).Ainsi,nous hoisissonsun ritèrebasésurlavarian edel'angle

de appour hoisirletempsd'intégration.

Soit

σ

2

θ,max

la varian e maximale tolérée sur l'angle de ap. Le ara tère additif des

matri esdevarian es- ovarian esfaitquelavarian ede l'anglede apàl'instant

t

(notée

σ

2

θ,t

)s'é rit:

σ

2

θ,t

=

t

X

i=1

Q

i

(3, 3) = t · Q

1

(3, 3)

(

Q

i

(3, 3)

estinvariantdansletemps) (54)

Aunal, on hoisit

t = E



_σ

2

θ,max

Q

1

(3,3)



(où

E(x)

désignelapartieentièrede

x

).

Onpeutalorsappliquerl'algorithmedeGraphSLAMentrelesinstants

0

et

t

.Ensuite,la varian edu apàl'instant

t

diminuegrâ eàl'appli ationdel'algorithme.Soit

σ

2

θ−GSLAM,t

ettevarian e.Onané essairement

σ

2

θ−GSLAM,t

< σ

θ,t

2

, e quinouspermetd'initialiser un

nouveaumor eaudelatraje toireentre lesinstants

t

et

t

′

où

t

′

estdénipar:

t

′

= E







σ

2

θ,max

− σ

2

θ−GSLAM,t



Q

1

(3, 3)





(55)

La nouvelleinitialisation pour l'appli ationdu GraphSLAM entre les instants

0

et

t

′

sera

la on aténation du résultat du GraphSLAM entre les instants

0

et

t

et l'in rément de traje toireobtenuenintégrantleséquationsdemodèleentrelesinstants

t

et

t

′

.