Chapitre 10 Localisation

Ce chapitre prsente les principales methodes de localisation. On pourra trouver une description succinte dun grand nombre dautres mthodes dans [35].

10.1

Diffrentes capacits de localisation

Il existe trois types de capacits regroupes sous le terme localisation, de difcult diffrente. La premire, appele suivi de position, est la capacit de mettre jour une estimation existante de la position au vu de donnes proprioceptives ou de perceptions nouvellement acquises. Dans le cas des donnes proprioceptives, cette mise jour concerne un dplacement du robot et va en gnral diminuer la prcision de lestimation courante de la position, cause de lerreur sur la mesure. Dans le cas de perceptions, au contraire, cette mise jour va en gnral permettre damliorer cette estimation grce au lien avec lenvironnement fourni par ces donnes. Lutilisation de cet ancrage dans lenvironnement est fondamental pour assurer que lestimation de la position rete correctement la position du robot dans lenvironnement rel. Cette mise jour intgrant les deux types de donnes permet de combiner les avantages inhrents aux deux types dinformation an destimer au mieux la position du robot. En pratique, toutefois, le suivi de position est problmatique car il repose sur une estimation initiale de la position qui doit souvent tre fournie par une source extrieure. De plus, si la position estime scarte trop de la position relle du robot, il peut trs bien tre impossible de parvenir corriger lerreur et de retrouver la position relle, ce qui conduit une drive de lalgorithme. La seconde capacit de localisation, la localisation globale, est plus gnrale et permet de retrouver la position du robot sans quaucune estimation initiale ne soit fournie. Cette capacit est trs importante du point de vue de lautonomie, car elle permet au robot de trouver sa position initiale, dans toutes les conditions, 103

104

CHAPITRE 10. LOCALISATION

sans intervention extrieure. Elle permet, par exemple, de couper lalimentation dun robot des ns de maintenance, puis de remettre ce robot dans une position quelconque de lenvironnement sans se soucier dinitialiser correctement son estimation de la position. Enn la troisime capacit est la capacit retrouver la position dun robot kidnapp, cest dire dun robot dont on a une estimation de la position, mais dont lestimation est fausse car il a t dplac, sans que le systme de localisation nen soit inform. Par rapport la localisation globale, ce cas prsente la difcult supplmentaire de parvenir dtecter que la position actuellement suivie nest plus correcte. Cette phase est dlicate car il faut distinguer entre les cas o les perceptions sont simplement temporairement bruites, sans que le robot ait t dplac, et les cas o le robot a rellement t dplac. Les capacits de suivi de position et de localisation globale ont des proprits duales. Comme le note Piasecki [81], dans le contexte dune carte mtrique, le suivi de position est une mthode locale, continue, qui effectue rgulirement de petites corrections lestimation de la position du robot. Cette mthode effectue de telles corrections en se basant sur des objets de lenvironnement et la manire dont ils ont t perus par le robot. Lidentication de ces objets est de plus simplie grce lestimation initiale de la position qui permet, en cas de perceptual aliasing, de dcider quel est lobjet qui a t peru parmi les diffrents objets correspondant aux perceptions. Au contraire, la localisation globale est une mthode globale, discontinue, qui effectue exceptionnellement des corrections de grande ampleur de la position estime. Sa premire tche, avant destimer une position, est de dterminer quels objets de lenvironnement correspond chacune des perceptions du robot. Cette tche peut tre trs simple dans le cas o la carte ne contient que des amers diffrents, mais est en gnral assez complexe car plusieurs amers sont identiques cause du perceptual aliasing. Comme nous lavons soulign, le suivi de position permet de fusionner et de tirer parti des informations proprioceptives et des perceptions disponibles pour le robot. Cette mthode ne conduit toutefois qu une estimation qui est localement la meilleure approximation possible de la position. En effet, la recherche est contrainte par lestimation prcdente de cette position. La position estime sera donc celle qui est la plus en accord avec les donnes recueillies, dans le voisinage de cette estimation prcdente. Lestimation rsultante peut donc trs bien ne pas correspondre la position qui, sur lensemble de la carte, correspond le mieux aux donnes. En principe, la localisation globale permet une telle estimation optimale. Au niveau de lutilisation des donnes disponibles pour le robot, il existe de nouveau

10.2. ESTIMATION DE LA POSITION PAR LES PERCEPTIONS

105

deux classes de mthodes de localisation globale : La premire, qui ne fonctionne que dans des environnements o il nexiste aucun perceptual aliasing, fait appel uniquement aux perceptions disponibles en une position donne. Nous lappellerons dans ce cours infrence directe de position. La seconde, qui fonctionne dans tous les environnements, fusionne au contraire les informations proprioceptives et les perceptions, comme le fait le suivi de position. Toutefois, au lieu de restreindre la recherche par une estimation prcdente de la position, elle estime parmi toutes les positions possibles au sein de la carte celle qui correspond le mieux aux donnes prsentes et passes recueillies par le robot. Les mthodes de cette catgorie reposent, dune faon ou dune autre, sur le suivi de plusieurs hypothses de position, ce qui permet de gnraliser le suivi de position en utilisant de manire plus efcace les informations disponibles. Dans la suite de ce chapitre, nous allons prsenter plus prcisment les diffrentes mthodes de localisation. Nous allons dabord voir comment il est possible destimer la position dun robot au vu des seules perceptions. Puis, dans le cas de systmes perceptifs soumis au perceptual aliasing, nous verrons comment il est possible dintgrer les informations proprioceptives, de manire locale puis globale, an de lever les ambiguts restantes.

10.2

Estimation de la position par les perceptions

Dans cette section, nous rsumons les diffrentes mthodes qui peuvent tre utilises pour estimer la position dun robot laide des seules perceptions. Dans le cas o lenvironnement est exempt de perceptual aliasing , cette tape suft dterminer la position du robot de manire unique. Cette mthode est alors la premire mthode de localisation globale mentionne prcdemment. Dans le cas o le perceptual aliasing est prsent, ces mthodes sont galement utilises mais elles serviront reprer plusieurs positions possibles pour le robot au sein de lenvironnement. Le suivi de position ou la seconde mthode de localisation globale qui seront prsents dans les paragraphes suivants doivent alors tre utiliss en sus pour slectionner la position correcte.

10.2.1

Cartes topologiques

Dans le cas des cartes topologiques, estimer la position partir des seules perceptions est extrmement simple. En effet, parmi tous les lieux reprsents dans la carte, la position du robot est celle dun des nuds qui correspond le mieux aux

106

CHAPITRE 10. LOCALISATION

perceptions courantes. La recherche de ce nud passe donc par la comparaison des perceptions du robot avec les perceptions mmorises dans chacun des nuds de la carte. Les nuds qui sont identiques ou sufsamment similaires sont alors reconnus comme positions possibles du robot. En labsence de perceptual aliasing, tous les nuds de la carte correspondent des situations diffrentes. Cette tape est alors sufsante pour la localisation complte du robot car le nud reconnu est unique. Diffrents systmes perceptifs ont t utiliss pour implanter de tels modles. Certains auteurs utilisent des images panoramiques de lenvironnement pour dnir les nuds de la carte [58, 38]. Dautre modles utilisent les directions ou les distances damers ponctuels tous discernables, soit en simulation [18, 67, 101, 89], soit sur des robots rels [7, 40]. Lutilisation dimages panoramiques permet par exemple de raliser un systme de localisation partir dune mthode dindexation dimages. Il suft en effet davoir une base de donnes indexant les images des diffrents nuds de la carte, puis, pour se localiser, de rechercher dans cette base limage la plus proche de limage courante. Cette image nous donnera le nud correpondant la position courante. Lindexation peut par exemple utiliser une analyse en composantes principales qui va dterminer une base sur laquelle il sera possible de projetter chaque image. Les coordonnes de chaque images dans cette base fournissent ainsi un reprsentation de faible dimension de chaque nud de la carte. Pour la localisation, il suft de projetter limage courante sur la base et de chercher limage ayant les coordonnes les plus proches. Lorsquune position dans un espace mtrique est associe chacun des nuds de la carte, la localisation permet en outre de dterminer la position mtrique du robot. Cette position peut simplement tre la position du nud reconnu, mais il est souvent possible dobtenir une prcision supplmentaire. En effet, au lieu de tenir simplement compte du nud le plus conforme aux perceptions courantes, il est possible de tenir compte de chacun des nuds, selon son degr de similarit avec ces perceptions. La mthode mise en uvre dans de tels modles pour raliser cette estimation de position est le codage par population de vecteurs [42]. Cette mthode consiste estimer la position du robot par la moyenne des positions des diffrents nuds, pondres par le degr de similarit de chaque nud avec les perceptions du robot. Cette mthode donne une estimation prcise de la position du robot, mais suppose une relative continuit de lenvironnement. Elle suppose en effet que des lieux similaires seront proches les uns des autres pour que la moyenne des positions ait un sens. Les perceptions doivent donc varier de manire relativement continue avec la position. Lorsque les modles permettent la gestion du perceptual aliasing (par une des mthodes dcrites dans les paragraphes suivants), les lieux peuvent galement tre dnis par des images panoramiques de leur environnement [4, 30, 49, 103, 83, 104], ou par la conguration des positions damers distants [9, 99]. Mais,

10.2. ESTIMATION DE LA POSITION PAR LES PERCEPTIONS

107

puisque le perceptual aliasing sera gr par ailleurs, des dnitions plus simples des nuds peuvent galement tre adoptes, au prix dune moins grande discrimination. Certains modles utilisent ainsi les valeurs brutes de capteurs de distance [77, 71, 60, 51, 49, 80], ou la conguration des murs autour du robot an de dtecter des angles de couloirs ou des embranchements [61, 24, 90, 20, 78, 92, 94].

10.2.2

Cartes mtriques

Dans le cas des cartes mtriques, diverses mthodes destimation de la position existent. Lorsque les perceptions sont constitues damers ponctuels, une mthode de triangulation peut tre utilise [10, 45, 67, 70]. Cette mthode repose sur la mesure de la direction et de la distance damers ponctuels connus. La perception de trois amers de ce type permet en effet de dnir la position du robot de manire unique. Un simple calcul mathmatique permet donc de calculer cette position partir des positions des amers. Ce calcul peut galement tre approch par des rseaux de neurones, [82], ou par des mthodes heuristiques qui permettent une meilleure rsistance au bruit [105]. Lorsque cette mthode est utilise avec des cartes ne comportant pas de perceptual aliasing, chaque amer est unique et cette mthode permet destimer directement de manire non ambigu la position du robot. En cas de perceptual aliasing, certains amers ne peuvent tre distingus et il faut tenir compte de lestimation prcdente de la position an de pouvoir identier correctement les diffrents amers et estimer correctement la position.
Point Ligne Objet bidimentionnel

Perception

Positions possibles

F IG . 10.1: Les amers que peut reprer un robot fournissent plus ou moins dinformation sur sa position en 2 dimensions. La dtection dun amer ponctuel permet de savoir que le robot se trouve sur un cercle entourant lamer. Un amer rectiligne permet de connatre la distance du robot perpendiculairement cet amer, mais pas sa position le long de cet amer. Enn, un amer ayant une tendue spatiale en deux dimensions permet de dnir la position du robot de manire unique.

Certains types dobjets fournissent plus dinformation que des amers ponc-

108

CHAPITRE 10. LOCALISATION

tuels, sans toutefois permettre une estimation non ambigu de la position. Par exemple, cest le cas des murs dont la perception fournit une information sur la distance du robot ce mur, mais pas sur sa position le long de ce mur (cf. gure 10.1). Certains modles utilisent de tels types dobjets, qui permettent dafner une estimation prcdente de la position, mais pas destimer directement cette position [21, 53, 84]. Lorsque les objets mmoriss dans la carte ont une certaine tendue spatiale en deux dimensions, il est par contre possible dutiliser la perception dun seul objet an destimer directement la position du robot. Les amers utiliss peuvent alors tre des objets tridimensionnels dtects par une camra [91], les angles des obstacles dtects par un tlmtre laser [5, 12, 43, 53] ou un capteur ultrason [66, 84], des segments dtects en utilisant une camra [6] ou un tlmtre laser [21, 23, 75].
Position

Carte locale

Carte globale

Comparaison

F IG . 10.2: Pour estimer la position dun robot, il est possible de construire une carte locale reprsentant lenvironnement proche de la position courante. La comparaison de cette carte locale et de la carte globale de lenvironnement permet alors de trouver la position.

Dautre modles, enn, nestiment pas directement la position du robot au vu des perceptions, mais reposent sur la comparaison dune carte mtrique locale avec la carte mtrique globale (cf. gure 10.2). La carte mtrique locale est construite soit partir des seules perceptions courantes, soit partir des donnes proprioceptives et des perceptions recueillies sur un court laps de temps. Le problme est alors de trouver la portion de carte globale qui correspond le mieux la carte locale. Cette mthode est trs souvent utilise avec les grilles doccupation [79, 85, 88, 95], ainsi quavec des donnes brutes de tlmtres laser [68, 48, 31]. Le polygone de visibilit, qui entoure la zone despace libre visible depuis la position courante du robot peut aussi tre utilis [46, 54]. Comme nous le verrons dans la section suivante, ces mthodes sont souvent utilises sur un espace de recherche restreint par une estimation initiale de la position. Elles peuvent cependant tre utilises pour la localisation globale [79, 46, 54].

10.2. ESTIMATION DE LA POSITION PAR LES PERCEPTIONS

109

10.2.3

Corrlation de cartes

Dans cette section, nous allons dtailler une mthode de corrlation de cartes qui permet de chercher, pour deux cartes de dimensions rduites, la transformation (translation+rotation) qui permet la meilleure superposition. Cette transformation permet alors de corriger lestimation de position du robot. Cette mthode peut tre utilise avec diffrents types de cartes, dont les grilles doccupation, mais donne des rsultats particulirement efcaces avec des donnes issues dun tlmtre laser (Figure 10.3).

F IG . 10.3: Exemple de rsultat de corrlation de mesures laser. La partie suprieure montre les mesures afches la position mesure par lodomtrie. La partie infrieure montre le rsultat de la corrlation.

La mthode que nous prsentons ici est une mthode simple qui est relativement robuste et rsistante au bruit. Elle est base sur les histogrammes des directions des tangentes la mesure. La premire tape consiste calculer pour chaque point du scan la droite tangente en utilisant la mthode des moindres carrs. On cherche pour cela la droite qui fournit la plus faible erreur quadratique sur un ensemble comprenant quelques points avant et quelques points aprs le point courant (Figure 10.4).

110

CHAPITRE 10. LOCALISATION

D
ej

i
e2

e1

F IG . 10.4: Illustration de la mthode de calcul de la tangente en chaque point du scan. Pour chaque point i, on cherche les paramtres de la droite qui donnent la plus faible erreur quadratique e2 j.

On construit ensuite lhistogramme des directions de ces tangentes, aprs avoir ltr les points pour lesquels la qualit de lapproximation des tangentes est trop faible. Cet histogramme contient alors des pics lorsque des ensembles de points correspondent un mur rectiligne de lenvironnement. Aprs avoir construit ces histogrammes de direction pour les deux scans, on cherche le dcalage de ces histogrammes qui fournit la meilleure corrlation. Dans lhypothse o les deux scans ont t perus en des points de lenvironnement assez proches et reprsentent donc a peu prs la mme zone, ce dcalage correspond la transformation qui aligne les deux scans (Figure 10.5). Une fois la direction recale, pour corriger la translation, on projette les points selon la direction principale des tangentes. On construit ensuite lhistogramme du nombre de points projets sur la perpendiculaire cette direction et, en cherchant le maximum de corrlation entre les histogrammes correspondant aux deux scans, on corrige le dcalage en translation selon cette direction (Figure 10.6). On recommence ensuite cette procdure dans la direction perpendiculaire. Cette mthode fonctionne bien ds quil y a des structures rectilignes dans lenvironnement qui conduisent des histogrammes contenant des pics pour lesquels le recalage par corrlation fonctionne bien. Ils faut toutefois prendre un certain nombre de prcautions qui ne sont pas dtailles ici, notamment faire un ltrage intelligent des scan an de ne garder que les points qui ne correspondent ni du bruit, ni des lments dynamiques. Il faut galement veiller ne raliser ce recalage que pour des scans qui ont t perus des positions effectivement proches, sous peine de recueillir des rsultats trs fantaisistes. Pour sassurer de ce point, il est possible de vrier la qualit de la corrlation des histogrammes an de vrier a posteriori que les deux scans reprsentaient des portions similaires de lenvironnement.

10.2. ESTIMATION DE LA POSITION PAR LES PERCEPTIONS

111

F IG . 10.5: Illustration de la mthode de recalage par corrlation dhistogrammes.

dx

dx

F IG . 10.6: Illustration de la mthode de recalage de la translation par corrlation dhistogrammes.

112

CHAPITRE 10. LOCALISATION

10.2.4

Limitations de lestimation de la position par les perceptions

Lhypothse dun environnement sans perceptual aliasing est relativement forte, car beaucoup de capteurs en robotique sont limits et fortement bruits. De plus, les environnements intrieurs, de type bureaux, peuvent tre trs rguliers et prsenter de nombreuses zones apparemment similaires pour le robot. Toutefois, les environnement courants contiennent souvent sufsamment dinformations accessibles des capteurs prcis et efcaces. Un tre humain, par exemple, na aucun mal se reprer dans un immeuble de bureaux, en lisant les numros crits sur les portes (trouver son chemin jusqu la sortie est un autre problme !). Il est donc thoriquement possible de concevoir des systmes sufsamment discriminants pour tre capables de se reprer grce aux seules perceptions et une carte prcise. Il est de plus possible damnager lenvironnement an de simplier la tche de perception pour le robot (comme le montre lexemple des numros de porte). Cette solution nuit toutefois fortement lautonomie du robot puisquil est alors limit aux environnements bien dnis qui ont t prpars lavance. Le rejet de cette dernire solution et la difcult de raliser des capteurs sufsamment discriminant conduit donc la plupart des systmes de navigation robotique prendre en compte le perceptual aliasing et utiliser les donnes proprioceptives pour dterminer leur position de manire unique. Nous allons dcrire les diffrentes mthodes qui peuvent tre utilises dans la suite de ce chapitre.

10.3

Suivi dune hypothse unique

Lorsque les perceptions ne sufsent pas pour estimer la position de manire unique, une seconde source destimation de la position du robot est ncessaire pour lever lambigut. Cette seconde estimation provient, dune part de la position dtermine lors de la prcdente phase de localisation et, dautre part, des donnes proprioceptives recueillies depuis cet instant. Les mthodes prsentes dans cette section utilisent cette seconde estimation pour slectionner ou calculer, chaque instant, la position qui est la plus cohrente vis--vis de cette estimation. Les positions estimes grce aux perceptions qui ne sont pas compatibles avec la position prcdente sont simplement ignores.

10.3.1

Cartes topologiques

Dans une carte topologique, slectionner le nud correct parmi les nuds correspondant aux perceptions peut reposer simplement sur ladjacence avec le nud prcdent. Dans ce cas, le nud slectionn est celui qui est connect au

10.3. SUIVI DUNE HYPOTHSE UNIQUE

113

nud reprsentant la position prcdente. Cette information est toutefois rarement sufsante et les relations mtriques mmorises dans les artes entre nuds sont souvent utilises en complment. Le nud slectionn est donc celui dont la position relative par rapport au nud prcdent correspond le mieux aux donnes proprioceptives [60, 61, 24, 77]. Lorsquune position mtrique est associe chaque nud, cest le nud dont la position est la plus proche de la position estime par lodomtrie qui est slectionn [9, 62, 106]. Certains modles fonctionnent dans le sens oppos. Au lieu dutiliser les donnes proprioceptives pour slectionner un nud parmi les nuds possibles, ils utilisent ces donnes pour restreindre lensemble des nuds possibles et utilisent ensuite les perceptions pour slectionner le nud correct parmi ceux-ci. Les perceptions sont, par exemple, utilises pour choisir un nud parmi tous les nuds adjacents au nud prcdent [103], ou parmi les nuds sufsamment proches de la position estime par lodomtrie [104]. Enn, certains modles intgrent les deux tapes en une seule. Un paramtre mesure la probabilit que chaque nud reprsente la position courante. Ce paramtre intgre, dune part, la similarit du nud avec les perceptions courantes, et dautre part sa proximit avec la position estime par lodomtrie. Le nud ayant la plus forte probabilit peut alors tre reconnu [71], ou la position peut tre estime par codage par population de vecteurs en utilisant les probabilits calcules [4, 99].

10.3.2

Cartes mtriques

Dans une carte mtrique, lestimation initiale de la position est utilise pour restreindre lespace de recherche de la position correspondant aux perceptions. Dans le cas o la carte contient des objets, une estimation de la position permet de simplier le problme de lappariement. En effet, dans le cas o les senseurs sont soumis un fort perceptual aliasing, de nombreux objets identiques, situs des positions diffrentes, sont prsents dans la carte. Lorsque le robot peroit un objet, dterminer quel objet a t peru exige dexaminer un grand nombre de possibilits. Lestimation de la position du robot permet donc destimer la position des objets perus et donc de dterminer quels objets de la carte ils correspondent. Ce choix se fait en gnral en appariant simplement chaque objet peru lobjet mmoris identique le plus proche [6, 21, 23, 29, 43, 66, 75, 93, 105]. Une fois lappariement effectu, les objets sont identis sans ambigut et permettent donc destimer la position de manire unique. Lorsque la position est dtermine par la mise en correspondance dune carte locale et dune carte globale, la position estime est utilise pour restreindre la recherche de la position donnant la meilleure correspondance entre les deux cartes [85, 88, 95, 108]. La recherche du maximum de correspondance est simplement

114

CHAPITRE 10. LOCALISATION

effectue sur une zone limite autour de la position estime prcdemment. La zone tant de faible tendue, le problme de perceptual aliasing ne se pose plus et la recherche conduit une position unique. Lorsque la position correspondant aux perceptions a t identie de manire unique, elle peut tre considre directement comme la nouvelle estimation de la position du robot [108, 43, 105]. Cependant la plupart des modles considrent que cette estimation est entache derreur, de la mme manire que lestimation initiale. La nouvelle position du robot est donc en gnral une combinaison de ces deux positions. La plupart des modles [6, 93, 75, 23, 66, 85, 10, 12, 68, 21] utilisent un ltre de Kalman [72] pour raliser cette combinaison. Ce ltre permet de calculer une estimation optimale de la position du robot, connaissant les deux positions et leurs variances respectives. Il constitue une mthode classique de localisation et est dcrit en dtails dans la section 10.3.3. Dautres mthodes sont galement utilisables pour combiner ces deux informations, par exemple la minimisation dune fonction de cot relie ces deux positions [95], ou lutilisation de la mthode des moindres carrs rcursifs [11].

10.3.3
Principe

Le ltrage de Kalman pour la localisation

Le ltre de Kalman permet destimer ltat dun systme partir dune prdiction bruite de son volution et de mesures bruites de cet tat. Cest un ltre rcursif optimal, qui suppose que le systme considr est linaire et les bruits blancs (de moyenne nulle). Pour la localisation en robotique mobile, ltat du systme est la position du robot, la prdiction de lvolution proviendra des donnes odomtriques et les mesures proviendront des perceptions, qui permettent de calculer la position grce la carte. Dans la suite, nous prsentons succintement la description mathmatique du ltre avant de donner un exemple dapplication. t de la valeur de ltat Xt du Le ltre donne chaque instant une estimation X systme, ainsi quune estimation de la prcision de cette estimation sous forme de sa matrice de covariance Pt 1 . Lvolution de ltat du systme est modlise par lquation linaire suivante : Xt +1 = A.Xt + B.ut + evo (10.1)

o A et B sont des matrices, ut est le vecteur des commandes donnes au robot et evo est le bruit sur lestimation de ltat, suppos desprance nulle et de variance Q = E {evo T evo }.
t = Xt + B, la on note le bruit comme une variable alatoire B de moyenne nulle, avec X T T matrice de covariance est donnes par Pt = E {B.B } = E {(X X )(X X ) }. En pratique, B et X sont inconnus, mais le ltre de Kalman fournit directement une estimation de Pt
1 Si

10.3. SUIVI DUNE HYPOTHSE UNIQUE

115

Une mesure Yt effectue sur ltat du systme sera donne par lquation linaire : (10.2) Yt = H .Xt + obs ou H est la matrice dobservation et obs le bruit de mesure, suppos de moyenne nulle et de variance PY = E {obs T obs }.

Prdiction

X * PX* Mise jour ^ X PX

Observation

Y PY

F IG . 10.7: Schma de fonctionnement du ltre de Kalman.

Le fonctionnement du ltre se droule en quatre tapes (Figure 10.7) : Prdiction de ltat linstant courant X , ainsi que de sa covariance Pt partir du modle dvolution, de lestimation au pas de temps prcdent et de la commande depuis cet instant : t 1 + B.ut Xt = A.X La covariance est galement prdite par la formule : t 1 .AT + Q Pt = A.P (10.4) (10.3)

Prdiction de lobservation partir du modle dobservation et de lestimation de ltat : (10.5) Yt = H .Xt Observation de ltat : on obtient, grce au systme perceptif, une mesure Y , dont on estime le bruit PY grce au modle du processus de perception. Correction de ltat prdit par mise jour proportionnellement lerreur entre lobservation prdite et lobservation ralise : t = Xt + K (Y Yt ) X t = Pt KHPt P (10.6) (10.7)

ou K est le gain de Kalman, calcul pour minimiser lerreur destimation au sens des moindres carrs et donn par la formule : K = Pt H T .(H .Pt .H T + PY )1 (10.8)

Ces quatre tapes sont utilises chaque nouvelle information, soit de dplacement, soit de perception, an de mettre jour lestimation de ltat du systme.

116 Application dans le cas mono variable

CHAPITRE 10. LOCALISATION

Prsentons lapplication de ce ltre dans un cas trivial : le cas o ltat du systme est dcrit par une variable scalaire X = x, de variance Pt = x 2 . Si on suppose que lobservation permet dobtenir directement la valeur de ltat : Yt = y = x avec une variance PY = y 2 , le gain du ltre scrit simplement :
2 x K = 2 x + y 2

et lquation de mise jour devient :

2 x (y x ) x = x + 2 2 x + y 2 2 x y + y x = 2 2 x + y

(10.9) (10.10)

La mise jour revient alors faire une moyenne pondre par la variance de la prdiction et de lobservation. Cette moyenne donne plus dimportance la valeur ayant la variance la plus faible et donc la plus able (Figure 10.8).
1 x* = 10
2 = 10 x*

2 = 50 y = 20 y ^ = 11.66 2 = 8.33 x ^ x

0 0 x*

^ x

40

F IG . 10.8: Illustration du ltre de Kalman dans le cas mono variable. La valeur estime est la moyenne des valeurs prdites et observes pondres par leurs variances.

Intuitivement, le ltre de Kalman va donc accorder plus dimportance aux valeurs pour lesquelles lincertitude est la plus faible et les privilgier lors de la mise jour. En pratique ces variances sont souvent bases sur des estimations empiriques (notamment en robotique pour lodomtrie et les capteurs). Il faut donc faire trs attention ne pas sous-estimer ces incertitudes de mesure, car, dans

10.3. SUIVI DUNE HYPOTHSE UNIQUE

117

ce cas, le ltre de Kalman convergerait vers ces mesures, ce qui peu conduire une divergence du ltre si ces mesures ne sont pas rellement aussi able que lestimation de covariance le laisse penser. Le ltrage de Kalman tendu Le ltre prsent dans la section prcdente suppose des quations dvolution et dobservation linaires, ce qui est rarement le cas en robotique mobile. Pour tendre le ltrage aux systmes non-linaires, le ltre de Kalman tendu linarise simplement les quations grce un dveloppement de Taylor. Partant des quations non linaires suivante : Xt +1 = f (Xt , ut ) + evo Yt = h(Xt ) + obs on utilise les matrices Jacobiennes A et H dnies par : fi x j hi Hi j = x j Ai j = (10.13) (10.14) (10.11) (10.12)

Avec ces deux matrices Jacobiennes, le principe du ltre de Kalman reste exactement le mme, en remplaant simplement les quations du ltre de Kalman original par les quations suivantes : Xt Pt Yt K t X t P = = = = = = f (Xt , ut ) t 1 .AT + Q A.P h(Xt ) Pt H T .(H .Pt .H T + PY )1 Xt + K (Y Yt ) Pt KHPt (10.15) (10.16) (10.17) (10.18) (10.19) (10.20)

Application la localisation sans perceptual aliasing Supposons, titre dexemple, un robot dont on peut commander la vitesse de translation v et de rotation . Ltat que lon cherche estimer est simplement sa position dans le plan : Xt = (x, y, ). Le vecteur de commande est ut = (v, ), ce qui conduit lquation dvolution du systme : x + v.dt .cos() f (Xt , ut ) = y + v.dt .sin() + .dt

118

CHAPITRE 10. LOCALISATION

Nous supposons de plus que le bruit entachant cette estimation est indpendant pour chaque variable et proportionnel aux vitesses : 0 0 T .v 0 T .v Q= 0 0 0 R . Supposons enn que le systme de perception permette de mesurer directement la position, par rfrence la carte. Lquation dobservation sera simplement : x h(Xt ) = y et nous estimons un bruit constant sur cette mesure : 0 O 0 Py = 0 O 0 0 0 O Les matrices Jacobiennes correspondant ces quations sont donc : 1 0 sin() A = 0 1 cos() 0 0 1 1 0 0 H = 0 1 0 0 0 1 Lalgorithme du ltre de Kalman tendu se simplie alors : Xt Pt Yt K t X t P = = = = = = f (Xt , ut ) t 1 .AT + Q A.P Xt Pt (Pt + PY )1 Xt + K (Y Xt ) Pt KPt

Pour linitialisation de lalgorithme, nous supposons connatre une estimation de la position du robot :

10.3. SUIVI DUNE HYPOTHSE UNIQUE x0 0 = y0 X 0 0 x0 0 0 = 0 y0 0 P 0 0 0

119

et nous appliquons les quations de mise jour pour chaque nouveau dplacement ou chaque nouvelle perception. Application la localisation avec perceptual aliasing En cas de perceptual aliasing, plusieurs valeurs de la mesure proviennent de la phase dobservation. Il faut donc choisir parmi ces valeurs la valeur correspondant la position relle du robot qui sera utilise pour la mise jour. Lorsque lobservation donne une mesure de la position, il est possible de slectionner simplement la mesure la plus proche de la position prdite pour le robot. Dans le cas gnral, il est prfrable dutiliser la distance de Mahalanobis , qui est une mesure de distance normalise par la covariance. Cette mesure permet par exemple de privilgier une mesure plus lointaine mais moins prcise qui aura en fait une probabilit plus grande de correspondre la mesure prdite (Figure 10.9).

Y2

Y1

F IG . 10.9: Illustration de lintrt de la distance de Mahalanobis. La mesure Y1 est plus proche de X en distance euclidienne mais les incertitudes font que ces mesures sont incompatibles. La distance de Mahalanobis sera plus faible pour Y2 et X du fait que les incertitudes montrent que ces mesures peuvent provenir de la mme variable.

Pour deux valeurs X et Y de covariances PX et PY , cette distance vaut : 1 d 2 = (X Y )T (PX + PY )1 (X Y ) 2

120

CHAPITRE 10. LOCALISATION

ce qui se traduit dans le cas scalaire par une simple pondration par les variances : d2 = (x y)2 2 2(2 x + y )

Dans le cas du ltrage de Kalman, cette distance est utilise entre lobservation prdite et les diffrentes observations faites sur le systme : 1 (Y Y )T (PY + PY )1 (Y Y ) 2 1 (H .X Y )T (H .Pt .H T + PY )1 (H .X Y ) = 2 partir des distances de Mahalanobis des diffrentes observations lobservation prdite, il est possible de slectionner lobservation la plus proche ou de choisir un seuil qui permettra de dterminer si une des observations correspond bien ltat courant. Si une des observation est en dessous de ce seuil, elle est utilise pour la mise jour du ltre, sinon, on considre que ltat na pas pu tre mesur et on ne fait pas de correction de la prdiction. Lune des principales faiblesses du ltre de Kalman pour la localisation provient prcisement de cette phase didentication (dappariement) des lments perus. En effet, en cas de mauvais choix d une mauvaise prdiction de la position du robot ou a une erreur de perception, lerreur destimation de la position sera conrme, voir augmente. Un tel processus conduit rapidement une divergence du ltre et une perte de la position du robot. d2 = Le ltre de Kalman unscented
Pt X Pt

X*

P* t X* P* t

F IG . 10.10: Illustration du fonctionnement du ltre de Kalman unscented.

Lorsque le systme nest pas linaire, plutt que de linariser pour prdire ltat et sa variance, il est possible dchantillonner ltat, dappliquer lquation dvolution chacun des chantillons puis de reconstruire ltat prdit et sa variance (Figure 10.10). Cette mthode donne en gnral de meilleurs rsultats que le ltre de Kalman tendu, mais est plus coteuse en terme de temps de calcul.

10.3. SUIVI DUNE HYPOTHSE UNIQUE

121

10.3.4

Limitations du suivi de position

Environnement rel 2 1 Perceptions 4 3

Donnes proprioceptives

Estimation de la position avec une estimation initiale correcte 2 4 3

Estimation initiale

Estimation de la position avec une estimation initiale incorrecte 2 4 3 1

Estimation initiale

F IG . 10.11: La position estime par le suivi de position dpend fortement de lestimation initiale de la position. Dans cet exemple, le robot mesure son dplacement dans un couloir (1 et 3) dans lequel il dtecte des portes (2 et 4) sans tre capable de les reconnatre individuellement (Partie a). Le systme de localisation va estimer la position du robot en intgrant ces donnes. Si lestimation initiale de la position est proche de la position correcte, le systme de localisation sera capable destimer prcisment la position relle du robot (Partie b). Toutefois, si lestimation initiale de la position est trop loigne de la position relle, le systme fournira une estimation de la position qui nest que localement optimale et ne correspondra pas la position relle (Partie c).

La limitation principale de toutes ces mthodes de suivi de position est quelles ne garantissent une bonne estimation de la position que localement, autour de lestimation initiale de la position. En pratique, si cette estimation initiale est trop loigne de la position relle, ces modles ne pourront pas estimer correctement la position du robot (cf. gure 10.11). Ces modles ne garantissent donc pas que la position calcule soit la position de la carte qui corresponde globalement le mieux aux donnes recueillies par le robot et donc la position relle la plus probable. Ce problme prend toute son importance lorsque lestimation de la position est perturbe la suite dinformations proprioceptives ou de perceptions errones. En effet, de telles informations errones peuvent faire diverger lestimation de la

122

CHAPITRE 10. LOCALISATION

position de telle manire que le systme soit par la suite incapable de retrouver une estimation correcte de la position. La position correcte du robot pourra cependant tre retrouve par lune des mthodes de localisation globale dcrites dans la section prcdente. Cette position pourra ensuite tre utilise comme nouvelle position initiale dans le processus de suivi de position. Toutefois, il est galement possible dutiliser une des mthodes de localisation globale dcrites dans le paragraphe suivant qui permettent de ne plus dpendre dune estimation initiale correcte de la position. Cette seconde solution, qui ne requiert pas lutilisation spare de deux mthodes de localisation est plus performante.

10.4

Suivi de plusieurs hypothses

La localisation globale, lorsque le robot est soumis au perceptual aliasing, ne peut se faire quen utilisant de manire optimale les informations proprioceptives et les perceptions. Contrairement au suivi de position qui utilise lestimation prcdente de la position pour slectionner lune des positions caractrise par les perceptions et ignorer les autres, il faut tenir compte chaque tape de toutes ces positions possibles. Ces positions conduisent des hypothses qui peuvent tre mises jour en fonction des donnes proprioceptives et quil faut comparer, an de choisir la plus pertinente chaque tape (cf. gure 10.12).

10.4.1

Suivi explicite de plusieurs hypothses

Ce suivi de plusieurs hypothses peut tre ralis de manire explicite, en grant une liste des hypothses en question. Lorsque des donnes proprioceptives sont disponibles, chaque hypothse est simplement mise jour de manire reter le dplacement du robot (cf. gure 10.12c). Lorsque de nouvelles perceptions sont disponibles, lensemble des positions de la carte susceptibles de correspondre ces perceptions est dtermin. Cet ensemble est ensuite compar lensemble des hypothses. Si une hypothse correspond une position perue, cette hypothse est alors mise jour en utilisant les perceptions par une mthode similaire celle permettant le suivi de position, par exemple un ltre de Kalman. Les positions perues qui ne correspondent aucune hypothse sont utilises pour crer de nouvelles hypothses associes la position correspondante. La crdibilit de chacune des hypothses est ensuite value, gnralement en fonction de la proximit de lhypothse avec une position correspondant aux perceptions du robot (cf. gure 10.12d). Ainsi, une hypothse verra sa crdibilit augmenter si elle est proche dune des positions correspondant aux perceptions du robot, elle la verra diminuer dans le cas contraire.

10.4. SUIVI DE PLUSIEURS HYPOTHSES

123

Environnement rel 1

Perceptions 3 2

Donnes proprioceptives

Utilisation de la perception 1

0,25

0,25

0,25

0,25

Hypothse de position

Crdibilit de lhypothse

Utilisation des donnes proprioceptives 2

0,25

0,25

0,25

0,25

Utilisation de la perception 3

0,1

0,05

0,8

0,05

F IG . 10.12: Le suivi de plusieurs hypothses permet de dterminer la position au sein de la carte qui globalement correspond le mieux aux donnes recueillies par le robot. Dans cet exemple, le robot est capable destimer son dplacement dans un couloir (2) dans lequel il dtecte des portes (1 et 3) sans tre capable de les reconnatre individuellement (Partie a). La perception dune porte sans aucune estimation pralable de la position permet simplement de crer plusieurs hypothses de position pouvant correspondre cette perception. Il est, ce stade impossible de dcider quelle hypothse est correcte (Partie b). Lintgration des donnes proprioceptives permet de mettre jour la position de chacune des hypothses, mais ne permet pas de les discriminer (Partie c). Des nouvelles perceptions permettent destimer la crdibilit relative de chacune des hypothses en rendant plus crdibles les hypothses dont la position correspond aux perceptions courantes et moins crdible les autres. (Partie d). Lhypothse ayant alors la plus forte crdibilit correspond la position qui rend le mieux compte des donnes recueillies par le robot.

124

CHAPITRE 10. LOCALISATION

De tels modles ont t implments en utilisant des cartes topologiques [27, 25] o les diffrentes hypothses correspondent diffrents noeuds de la carte. Il existe galement des modles de ce type utilisant des cartes mtriques [81, 53]. Ces derniers modles utilisent en gnral plusieurs ltres de Kalman en parallle et permettent de rsoudre en grande partie les problmes de divergence du ltre de Kalman employ seul.

10.4.2

Suivi implicite de plusieurs hypothses

Une seconde mthode de gestion dhypothses multiples permet dintgrer de manire similaire les deux types dinformations, mais ne gre pas explicitement les hypothses de position. Les diffrentes hypothses sont ici remplaces par une distribution de probabilit de prsence du robot sur lensemble des positions possibles de la carte. Cette reprsentation permet donc de considrer chacune des positions au sein de la carte comme une position possible du robot dont il faut valuer la crdibilit. Nous prsentons dabord le cadre gnral qui permet la gestion de cette distribution de probabilit, le ltrage Baysien, avant de voir comment il peut tre utilis en pratique. Le ltrage Baysien Le ltrage Baysien regroupe un ensemble de mthodes destimation dtat utilisant les probabilits et plus particulirement la loi de Bayes : P(X |Y ) = P(Y |X )P(X ) P(Y ) (10.21)

Dans le cadre de la localisation en robotique mobile, X est en gnral la position et Y une perception de lenvironnement. Cette loi nous permet donc destimer la probabilit P(X |Y ) des positions, connaissant une perception, cest dire prcisment ce que nous cherchons calculer pour localiser un robot. Pour ce calcul, nous aurons besoin de P(Y |X ), la probabilit dune perception connaissant la position, qui peut tre calcule grce la carte de lenvironnement et au modle du capteur utilis. Nous aurons galement besoin dune estimation des probabilits des positions P(X ) avant cette perception, ainsi que de la probabilit globale P(Y ) de cette perception. Dans un ltre Baysien, cette formule est utilise de manire rcursive et P(X ) est donc simplement lestimation prcdente de la probabilit des positions. Quant P(Y ), il peut tre calcul par un artice de calcul lors de lutilisation de la formule. En effet, par la loi des probabilits marginales : P(Y ) = P(Y |X )P(X )
X

10.4. SUIVI DE PLUSIEURS HYPOTHSES

125

ce qui permet dutiliser la loi de Bayes pour calculer les probabilits P(X |Y ) de la manire suivante : X , tempX |Y = P(Y |X )P(X ) 1 = X tempX |Y X , P(X |Y ) = .tempX |Y Dans cette quation, le terme P(X ) est la probabilit a priori (prior en anglais), P(X |Y ) est la probabilit a posteriori (posterior en anglais). La puissance de cette quation rside dans le fait quelle permet de transposer une quantit simple valuer, P(Y |X ), en une quantit plus difcile estimer et qui nous intresse, P(X |Y ). La vraisemblance P(Y |X ) est simple valuer car elle est le produit dun raisonnement causal : connaissant une carte, un modle de capteur et une position, on peut facilement prvoir les mesures que devraient renvoyer ce capteur. P(X |Y ), pour sa part, est le fruit dun raisonnement de diagnostic et il est difcile valuer car une perception Y ne permet pas de dnir simplement une position, mais peut correspondre plusieurs, notamment dans le cas du perceptual aliasing2 . Nous venons de voir comment la loi de Bayes permet de mettre jour une probabilit de position en fonction dune perception. Pour la localisation dun robot mobile, il faut galement pouvoir intgrer leffet dun dplacement sur une distribution de probabilit. Cela se fait aussi trs simplement grce lquation suivante (loi des probabilits marginales) : P(X |U ) = P(X |U , X )P(X )
X

(10.22)

Dans cette quation, P(X |U , X ) est un modle du dplacement du robot, qui donne la probabilit dune position X si le robot excute laction U dans la position X . Ce modle ne dpend que du robot et correspond souvent au modle dodomtrie que nous avons vu au dbut du cours. Comme prcdemment, la probabilit a priori, P(X ), est le fruit dune estimation ltape prcdente. Pour localiser un robot, nous cherchons videment estimer la position partir de nombreux dplacements et de nombreuses observations : P(xt |u1 , y2 , ..., ut 1 , xt 1 ). Pour pouvoir raliser les calculs de manire rcursive, lhypothse de Markov est utilise : on suppose que les perceptions ne dpendent que de ltat courant et que la position aprs un dplacement ne dpend que de
2 Dans le domaine mdical, de la mme manire, il est simple de prvoir de la vre (lobserva-

tion) si lon sait que lon a la grippe (ltat). Le raisonnement inverse est plus difcile car la vre peut correspondre plusieurs maladies.

126

CHAPITRE 10. LOCALISATION

la position prcdente. Ceci est illustr dans la gure 10.13 et correspond aux simplications suivantes : P(yt |xt , u1 , y2 , ..., ut 1 ) = P(yt |xt ) P(xt |u1 , y2 , ..., ut 1 , xt 1 ) = P(xt |ut 1 , xt 1 )
Yt-1 Ut-1 Yt Ut Yt+1

Xt-1

Xt

Xt+1

F IG . 10.13: Illustration des dpendances considres pour la localisation dun robot partir dune suite de perceptions et de dplacements. Une che indique que la valeur la pointe dpend de la valeur lorigine.

Partant de ces diffrents lments, le ltrage Baysien permet destimer de manire rcursive ltat dun systme partir dune estimation de son volution et de mesures sur cet tat. Pour pouvoir appliquer ce ltrage, nous avons besoin des lments suivants, qui sont tous connus ou qui peuvent tre dnis grce aux modles du robot, des capteurs et de lenvironnement : Un modle de capteur P(y|x) qui donne, pour une position x donne, la probabilit de la mesure y. Un modle daction P(x|u, x ) qui donne la probabilit que le robot arrive en x si il excute laction u en x . Une suite dactions et de perceptions u1 , y2 , ..., ut 1 , yt . Une estimation initiale de la position P0 (x), qui peut, par exemple, tre uniforme dans le cas de la localisation globale ou qui peut tre une rpartition gaussienne si nous connaissons la position initiale du robot. Le ltre permet destimer la position en fonction des donnes mesures : P(xt |u1 , y2 , ..., ut 1 , yt ), que nous noterons Bel (xt ) (de langlais Belief State). Une quation de mise jour rcursive permet alors destimer Bel (xt ) en fonction de Bel (xt 1 ). Cette quation se drive de la manire suivante : Bel (xt ) = P(xt |u1 , y2 , ..., ut 1 , yt ) (Bayes) = P(yt |xt , u1 , y2 , ..., ut 1 )P(xt |u1 , y2 , ..., ut 1 ) (Markov) = P(yt |xt )P(xt |u1 , y2 , ..., ut 1 ) ( probtotales) = P(yt |xt )
Z

P(xt |u1 , y2 , ..., ut 1 , xt 1 )P(xt 1 |u1 , y2 , ..., ut 1 )dxt 1

10.4. SUIVI DE PLUSIEURS HYPOTHSES (Markov) = P(yt |xt ) = P(yt |xt )

Z Z

127

P(xt |ut 1 , xt 1 )P(xt 1 |u1 , y2 , ..., ut 1 )dxt 1 P(xt |ut 1 , xt 1 )Bel (xt 1 )dxt 1

Ainsi, lestimation de ltat par un ltre Baysien correspond lutilisation de lquation de mise jour suivante, pour une perception yt et un dplacement ut 1 : Bel (xt ) = P(yt |xt )
Z

P(xt |ut 1 , xt 1 )Bel (xt 1 )dxt 1

(10.23)

Ce type de ltre regroupe un grand nombre dapproches connues sous des noms diffrents. Le ltre de Kalman est par exemple une implantation de ce ltre avec des distributions de probabilits gaussiennes et des modles linaires. Les Processus de dcision Markoviens Partiellement Observables et les ltres particulaires que nous allons voir par la suite en font galement partie. Reprsenter la position du robot par une telle distribution de probabilits permet dintgrer la totalit des informations recueillies au cours du temps. Elle est mise jour, dune part chaque dplacement du robot, et donc chaque nouvelle donne proprioceptive, et dautre part chaque nouvelle perception. De manire image, les donnes proprioceptives permettent de dplacer les probabilits dune position une autre pour reter le dplacement du robot. Les perceptions permettent de moduler les probabilits de chaque position. Ainsi, les positions pour lesquelles les perceptions prvues laide de la carte sont similaires aux donnes perues voient leurs probabilits augmenter, tandis que les autres voient leurs probabilits diminuer. Lorsquon utilise une telle distribution de probabilit, la position du robot calcule est en gnral donne par lhypothse ayant la plus forte probabilit [49, 51, 90, 92, 17, 37]. Cependant dautres estimations telles que la moyenne des positions pondres par leurs probabilits peuvent tre utilises [20]. Cette mthode est similaires au codage par population de vecteurs . La distribution de probabilits obtenue contient donc implicitement toutes les hypothses possibles sur la position du robot. La probabilit de chacune des positions est, de plus, estime en utilisant toutes les informations disponibles. Cette probabilit dpend faiblement des conditions initiales et peut donc tre initialise une distribution uniforme lorsquaucune information nest disponible sur la position du robot. La position sera alors retrouve, mme si le robot est soumis un trs fort perceptual aliasing, assurant ainsi la localisation globale du robot dans tous les environnements. Ces mthodes sont extrmement robustes en pratique et mettent en uvre un systme de localisation compltement autonome, ne dpendant daucune intervention extrieure [97].

128

CHAPITRE 10. LOCALISATION

Ces qualits reposent toutefois de manire importante sur le fait que la carte de lenvironnement est complte. En effet, les systmes de suivi de plusieurs hypothses ncessitent une estimation correcte des crdibilits des diffrentes positions possibles. Or une carte partielle de lenvironnement rend une telle estimation partir des perceptions difcile. Pour cette raison, ces systmes sont en gnral utiliss pour la localisation sur des cartes construites dans une phase pralable. Application dans le cas discret Pour appliquer algorithmiquement le ltrage Baysien que nous avons prsent dans le cas continu, il faut choisir une manire de reprsenter les distributions de probabilits. Une premire approche consiste discrtiser lenvironnement et donner chaque position discrte une probabilit approximant la valeur de la distribution continue. Cette approche a t utilise la fois avec des cartes topologiques o les nuds sont utiliss comme positions possibles, [20, 49, 51, 57, 78, 80, 90, 92, 94] et avec des cartes mtriques, pour lesquelles il est possible de discrtiser lensemble des positions, la manire des grilles doccupation [17, 37, 97].
Algorithm 10.1: Procdure UpdatePerception(Bel (xi ),y) ;
1: 2: 3: 4: 5: 6: 7: 8: 9:

=0 for all xi do Bel (xi ) = P(y|xi )Bel (xi ) = + Bel (xi ) end for for all xi do Bel (xi ) = Bel (xi )/ end for Return Bel (xi )

Algorithm 10.2: Procdure UpdateMouvement(Bel (xi ),u) ;

1: for all xi do 2: Bel (xi ) = xk P(xi |u, xk )Bel (xk ) 3: end for 4: Return Bel (xi )

Quelle que soit la discrtisation choisie, lalgorithme reste le mme. Il sagit dvaluer la probabilit Bel (xi ) que le robot soit situ sur ltat xi de la carte. Ceci est fait par deux procdures diffrentes selon que lon cherche intgrer

10.4. SUIVI DE PLUSIEURS HYPOTHSES

129

une perception ou des donnes proprioceptives. Pour une perception, on utilisera la procdure de lalgorithme 10.1. Les probabilits P(y|xi ) proviennent soit dun modle de capteur mtrique pour les grilles doccupation, soit dun modle comparant une perception avec les donnes mmorises dans les nuds pour la carte topologique. Pour un dplacement on utilisera lalgorithme 10.2. Limplantation nave de cet algorithme conduit une mise jour quadratique en fonction du nombre dtats (O(N)), ce qui peut tre rapidement lourd calculer. Cependant, le modle probabiliste de dplacement P(x|u, x ) est en gnral nul ds que lon sloigne de la position spcie par la commande u depuis ltat x . Si lon note p le nombre dtats pour lequel le modle est non nul, on peut facilement crire un algorithme en O(np), qui est donc linaire en le nombre dtats de la carte. Application dans le cas continu Il est galement possible dutiliser une autre mthode qui permette dapprocher une distribution de probabilit continue sur lespace de la carte [36] sans discrtisation de la carte. Cette mthode, le ltrage particulaire, constitue lune des mthodes actuellement les plus efcaces pour la localisation. Pour reprsenter la distribution de probabilit Bel (x), cette mthode utilise un chantillonage en un ensemble de particules qui permet dapprocher la distribution. En effet, considrons que la position du robot est la position moyenne dnie par la distribution de probabilit Bel (x) : Position =
P

x.Bel (x)dx

F IG . 10.14: Illustration de la mthode de reprsentation dune distribution de probabilit par des particules. Les particules sont tires alatoirement selon la loi reprsenter.

Supposons par ailleurs que lon soit capable de gnrer un ensemble de N chantillons de position i , que nous appellerons particules, selon la distribution

130

CHAPITRE 10. LOCALISATION

de probabilit Bel (x) (Figure 10.14). Par la loi des grands nombres, on peut alors approcher la position par : 1 Position i N i Cest cet ensemble de particules qui va permettre de reprsenter la distribution de probabilits de positions du robot. Tout le problme revient alors tre capable de gnrer rcursivement un ensemble de particules rparties selon la loi Bel (x). Dans notre cas, il est videment impossible a priori de gnrer de tels chantillons car Bel (x) est une distribution inconnue que nous cherchons valuer. Pour pouvoir nanmoins approcher cette fonction, nous allons introduire une fonction auxiliaire (x), appele fonction dimportance selon laquelle nous allons tirer nos particules. En effet, avec des telles particules, rparties selon la loi (x), il est galement possible dapprocher Bel (x). Pour cela, nous commenons par crire : Bel (x) = avec Bel (x) (x) = w(x)(x) (x) w(x) =

Bel (x) (x) et donc, si nous gnrons les chantillons alatoires i selon la fonction dimportance (connue car nous lavons choisie), nous pouvons estimer la position par : Position =
Z
x

x.Bel (x)dx =

x.w(x)(x)

1 w(i )i N i

Ainsi la distribution Bel (x) peut sapprocher sous la forme dun ensemble de particules tires selon une loi (x), chaque particule ayant un poids associ w(x) (Figure 10.15). La difcult dchantillonner selon Bel (x) reste cache dans la difcult de calculer w(x), mais grce un choix judicieux de (x) nous pouvons arriver rendre ce calcul possible en utilisant lquation de mise jour du ltrage Baysien. En effet, dans le cas qui nous intresse : Bel (xt ) = P(yt |xt )
R Z

P(xt |ut 1 , xt 1 )Bel (xt 1 )dxt 1

(10.24)

en choisissant (x) = P(x|ut 1 , xt 1 )Bel (xt 1 )dxt 1 , nous obtenons : w() = Belt () () R P(yt |) P(|ut 1 , x) Belt 1 (x)dx = R P(|ut 1 , xt 1 )Bel (xt 1 )dxt 1 = P(yt |)

10.4. SUIVI DE PLUSIEURS HYPOTHSES

P f

131

F IG . 10.15: Illustration de la mthode de reprsentation dune distribution de probabilit par des particules tires selon une loi dimportance. Les particules sont tires alatoirement selon la loi et ont chacune un poids associ w = f /.

Cette valeur est simple calculer car elle ne dpend que de la carte et du modle de capteurs pour la position dun chantillon . Reste a voir comment il est possible dchantillonner selon la loi (x) = R P(x|ut 1 , xt 1 )Bel (xt 1 )dxt 1 . Comme nous utilisons une mthode rcursive, Bel (xt 1 ) est approche sous la forme dun ensemble de particules t 1 , chacune associe un poids wt 1 . Lchantillonnage selon (x), se droule en deux tapes : une mise jour selon le modle dvolution P(x|ut 1 , xt 1 ), puis un chantillonage de particules suivant la loi Bel (xt 1 ). Les particules sont mises jour partir du modle de dplacement en tirant de manire alatoire, pour chaque particule i , une particule i selon la loi P(x|ut 1 , i ). Lensemble des i approche alors la distribution R P(x|ut 1 , xt 1 )Bel (xt 1 )dxt 1 , mais la fonction dimportance selon laquelle ils sont rpartis nest pas la bonne (Figure 10.16).

132

CHAPITRE 10. LOCALISATION

F IG . 10.16: Illustration de la mise jour de lensemble des particules en utilisant les dplacements.

10.4. SUIVI DE PLUSIEURS HYPOTHSES

wN w1 w2 v1 v2 w3 v 1 = w1 v 2 = w2 v 3 = w3 v 4 = w4 v 5 = w4 v 6 = w5 . . .

133

vi

w4

wi

F IG . 10.17: Illustration de la mthode dchantillonage. Lensemble de particules w est r-chantillonn en lensemble v.

Pour maintenir un chantillonage selon la fonction dimportance P(x|ut 1 , xt 1 )Bel (xt 1 )dxt 1 , il faut ensuite r-chantillonner, ce qui peut se faire selon une mthode appele Stochastic universal sampling. Cette mthode consiste choisir N particules i selon des points quirpartis sur un cercle sur lequel sont disposs lensemble initial des particules et leur poids (Figure 10.17). Cette mthode permet dchantillonner plus fortement aux positions qui ont des poids associs lev, et de rduire le nombre dchantillons aux positions de poids faibles (Figure 10.18).
R

Algorithm 10.3: Procdure Filtrage Particulaire(ti 1 ,wti 1 ,u,y) ;

1: 2: 3: 4: 5: 6: 7: 8: 9: 10: 11: 12: 13:

for all i do Tirer une particule i selon la loi P(x|u, ti 1 ) end for r-chantillonner les particules i selon les poids wti 1 . =0 for all i do Calculer le poids wti = p(y|i ) = + wti end for for all i do Normaliser les poids wti = wti / end for Return ti = i , wti

En rsum, la localisation par ltrage particulaire seffectue selon lalgorithme 10.3, et est illustr sur la gure 10.19. Cest un algorithme extrmement simple et

134
w

CHAPITRE 10. LOCALISATION

F IG . 10.18: Illustration de leffet de la mthode de r-chantillonnage.

particulirement robuste. Cette version de base demande cependant un rglage relativement prcis des paramtres, notamment des modles probabilistes de perception et de lodomtrie pour donner des rsultats satisfaisant. Le nombre de particules utiles pour approcher correctement la probabilit de position peut galement tre relativement dlicat choisir. Il existe dailleurs de nombreuses variantes et amliorations de la mthode, qui permettent par exemple de slectionner automatiquement le nombre de particules optimal pour fournir une approximation correcte de la position.

10.4. SUIVI DE PLUSIEURS HYPOTHSES

135

Position t-1

Particules t-1

Position t

Mise jour par le dplacement

R-chantillonage

Mise jour par les perceptions

F IG . 10.19: Illustration du fonctionnement de lalgorithme de ltrage particulaire.

136

CHAPITRE 10. LOCALISATION

10.5

Comparaison des mthodes de localisation

Le tableau de la gure 10.20 rsume les grandes caractristiques et compare les diffrentes mthodes de localisation prsentes dans ce cours. Topologique hypothse unique Modles de senseur Capacit de localisation Consommation Mmoire Consommation Calcul Simplicit dImplmentation Prcision Robustesse Quelconque Topologique hypothses multiples Quelconque Mtrique Filtre de Kalman Gaussien Mtrique Multi Kalman Gaussien Mtrique Filtres particulaires Quelconque

Suivi

Global

Suivi

Global

Global

++ + ++ +

++ ++ + ++

+ + + ++ -

+ ++ ++ +

++ ++ ++ ++ ++

F IG . 10.20: Comparaison des mthodes de localisation.

Les mthodes topologiques sont en gnral assez simples implanter puisquil sagit essentiellement de crer une procdure de comparaison de perceptions. Cependant, pour obtenir une robustesse correcte, cette procdure doit nanmoins tre faite avec soin et peut devenir complexe. La localisation directe a partir des seules perceptions est difcile envisager dans des environnements non triviaux. Lajout de suivi multi-hypothses dans ce cadre permet cependant damliorer la robustesse de manire assez simple. Les cartes topologiques conduisent cependant une localisation relativement imprcise et demandent un certain nombre de comportements sensori-moteurs efcaces pour les dplacements entre lieux. Un des gros inconvnients des cartes topologiques, comme nous le verrons plus loin, se situe plutt au niveau de leur cration, qui peut tre complexe automatiser. Concernant les mthodes mtriques, le ltre de Kalman connat un grand succs mais peut tre difcile rgler et risque de diverger assez frquement. Lutilisation de multi-Kalman permet damliorer sensiblement les choses mais conduit des implantations complexes et difciles maitriser. Les ltres particulaires sont une bonne solution, en particulier pour leur robustesse et leur simplicit dimplantation, mais peuvent tre difciles rgler et lourds en temps de calcul.