perception intelligente pour la navigation rapide de robots mobiles

HAL Id: tel-00673435https://tel.archives-ouvertes.fr/tel-00673435

Submitted on 23 Feb 2012

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

Larchive ouverte pluridisciplinaire HAL, estdestine au dpt et la diffusion de documentsscientifiques de niveau recherche, publis ou non,manant des tablissements denseignement et derecherche franais ou trangers, des laboratoirespublics ou privs.

Perception intelligente pour la navigation rapide derobots mobiles en environnement naturel

Florent Malartre

To cite this version:Florent Malartre. Perception intelligente pour la navigation rapide de robots mobiles en environ-nement naturel. Autre. Universit Blaise Pascal - Clermont-Ferrand II, 2011. Franais. .
https://tel.archives-ouvertes.fr/tel-00673435https://hal.archives-ouvertes.fr

No dOrdre : D.U 2132EDSPIC : 524

UNIVERSIT BLAISE PASCAL - CLERMONT II

COLE DOCTORALESCIENCES POUR LINGNIEUR DE

CLERMONT-FERRAND

T H S Epour obtenir le titre de

Docteur duniversit

Spcialit : VISION POUR LA ROBOTIQUE

Prsente par

Florent MALARTRE

Perception intelligente pour lanavigation rapide de robots mobiles

en environnement naturel

Thse dirige par Roland CHAPUISSoutenue publiquement le 16 Juin 2011 devant le jury :

M. Jean-Pierre DerutinProfesseur luniv. Blaise Pascal Prsident du juryM. Simon Lacroix Directeur de Recherche au LAAS RapporteurM. Karsten Berns Professeur lUniversit de Kaiserslautern (Allemagne)RapporteurM. Christophe Debain Charg de Recherche Cemagref Clermont ExaminateurM. Aurlien Godin Architecte la DGA ExaminateurM. Roland Chapuis Professeur luniv. Blaise Pascal Directeur de Thse

Remerciements

Les travaux prsents dans cette thse ont t raliss dansle cadre dunecollaboration entre le Lasmea et le Cemagref de Clermont-Ferrand. Je tiens donca remercier ces deux organismes qui mont permis de ralisercette thse dans lesmeilleures conditions. Je remercie tout particulirementMichel Dhome, directeurdu Lasmea et Anne Rizand, directrice du Cemagref de Clermont-Ferrand pouravoir mis ma disposition locaux et matriels. Je remercie galement la DGA(Direction gnrale de larmement) pour le financement et pour le suivi de cettethse.

Je tiens exprimer tout mes remerciements Simon Lacroix et KarstenBerns pour lintert quils ont port ma thse en acceptantden tre rapporteurset Jean-Pierre Drutin pour avoir prsid mon jury de thse.

Cette thse ne se serait pas aussi bien droul si elle navait pas t encadrepar des personnes aussi comptentes que sympathiques. Aussi je tiens remerciermon directeur de thse Roland Chapuis pour ses conseils aviss et son expertisedans le domaine. Je remercie aussi mon co-encadrant Christophe Debain pour sonimplication dans mes travaux et son suivi au jour le jour.

Cette thse sest galement bien droule grce au cadre dans lequel elle a teffectue. Pour cel je remercie chaleureusement lquipetechnique Gravir dulaboratoire Lasmea ainsi que lquipe TEAM du Cemagref pourtoute laide et labonne humeur que ces personnes mauront procures.

Je remercie aussi tous mes amis et collgues qui mauront permis de dcom-presser et de me dtendre pendant toute cette priode pour pouvoir mieux repartirpar la suite. Je tiens notamment remercier Pierre Delmas, mon partenaire detranche, pour les longs moments de dbat philosophique surles rpliques deMontaigne.

Comment crire des remerciements sans parler de sa famille.Jadresse donc unprofond remerciement mes parents et mes frres pour tout ces moments parfoisanodins qui mauront fait voyager hors du quotidien. Cest donc tout naturellementque je leur ddie cette thse.

ii

Rsum

Cette thse concerne la perception de lenvironnement pourle guidage automa-tique dun robot mobile. Lorsque lon souhaite raliser un systme de navigationautonome, plusieurs lments doivent tre abords. Parmi ceux-ci nous traiteronsde la franchissabilit de lenvironnement sur la trajectoire du vehicule. Cette fran-chissabilit dpend notamment de la gomtrie et du type de sol mais galement dela position du robot par rapport son environnement (dans unrepre local) ainsique lobjectif quil doit atteindre (dans un repre global).

Les travaux de cette thse traitent donc de la perception de lenvironnementdun robot au sens large du terme en adressant la cartographie de lenvironnementet la localisation du vhicule. Pour cela un systme de fusion de donnes est pro-pos afin destimer ces informations. Ce systme de fusion est aliment par plu-sieurs capteurs dont une camra, un tlmtre laser et un GPS. Loriginalit de cestravaux porte sur la faon de combiner ces informations capteurs.

A la base du processus de fusion, nous utilisons un algorithme dodomtrie vi-suelle bas sur les images de la camra. Pour accrotre la prcision et la robustesselinitialisation de la position des points slectionns sefait grce un tlmtrelaser qui fournit les informations de profondeur. De plus, le positionnement dansun repre global est effectu en combinant cette odomtrie visuelle avec les infor-mations GPS. Pour cela un procd a t mis en place pour assurer lintgrit delocalisation du vehicule avant de fusionner sa position avec les donnes GPS.

La cartographie de lenvironnement est toute aussi importante puisquelle vapermettre de calculer le chemin qui assurera au vhicule unevolution sans risquede collision ou de renversement. Dans cette optique, le tlmtre laser dj pr-sent dans le processus de localisation est utilis pour complter la liste courante depoints 3D qui matrialisent le terrain lavant du vhicule. En combinant la locali-sation prcise du vhicule avec les informations denses du tlmtre il est possibledobtenir une cartographie prcise, dense et go-localise de lenvironnement.

Tout ces travaux ont t expriments sur un simulateur robotique dvelopppour loccasion puis sur un vhicule tout-terrain rel voluant dans un mondenaturel. Les rsultats de cette approche ont montr la pertinence de ces travauxpour le guidage autonome de robots mobiles.

Mots-clefs : fusion de donnes, vision, perception, localisation, cartographie,robot mobile.

iii

Abstract

This thesis addresses the perception of the environment forthe automaticguidance of a mobile robot. When one wishes to achieve autonomous navigation,several elements must be addressed. Among them we will discuss the traversabilityof the environment on the vehicle path. This traversabilitydepends on the groundgeometry and type and also the position of the robot in its environment (in a localcoordinate system) taking into acount the objective that must be achieved (in aglobal coordinate system).

The works of this thesis deal with the environment perception of a robot inthe broad sense by addressing the mapping of the environmentand the locationof the vehicle. To do this, a data fusion system is proposed toestimate theseinformations. The fusion system is supplied by several low cost sensors includinga camera, a rangefinder and a GPS receiver. The originality ofthis work focuseson how to combine these sensors informations.

The base of the fusion process is a visual odometry algorithmbased on cameraimages. To increase the accuracy and the robustness, the initialization of theselected points position is done with a rangefinder that provides the depth informa-tion. In addition, the localization in a global reference ismade by combining thevisual odometry with GPS information. For this, a process has been established toensure the integrity of localization of the vehicle before merging its position withthe GPS data.

The mapping of the environment is also important as it will allow to computethe path that will ensure an evolution of the vehicle withoutrisk of collision oroverturn. From this perspective, the rangefinder already present in the localizationprocess is used to complete the current list of 3D points thatrepresent the field infront of the vehicle. By combining an accurate localizationof the vehicle withinformations of the rangefinder it is possible to obtain an accurate, dense andgeo-located map environment.

All these works have been tested on a robotic simulator developed for thispurpose and on a real all-terrain vehicle moving in a naturalworld. The results ofthis approach have shown the relevance of this work for autonomous guidance ofmobile robots.

Keywords : data fusion, vision, perception, localization, mapping,mobilerobot.

Table des matires

Introduction Gnrale xv

1 Problmatique : la navigation autonome 11.1 Cahier des charges. . . . . . . . . . . . . . . . . . . . . . . . . 21.2 La perception pour laction. . . . . . . . . . . . . . . . . . . . . 5

1.2.1 Linfluence de la perception sur le guidage. . . . . . . . 61.2.2 Linfluence du guidage sur la perception. . . . . . . . . . 6

1.3 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6

2 Localisation dun vhicule par fusion Camra-Tlmtre 72.1 Introduction Gnrale. . . . . . . . . . . . . . . . . . . . . . . . 72.2 Techniques de localisation pour vhicules autonomes. . . . . . . 7

2.2.1 Localisation absolue. . . . . . . . . . . . . . . . . . . . 82.2.1.1 Localisation absolue par GPS. . . . . . . . . . 82.2.1.2 localisation absolue par SIG. . . . . . . . . . . 8

2.2.2 Localisation relative. . . . . . . . . . . . . . . . . . . . 92.2.3 Fusion de localisation absolue-relative. . . . . . . . . . . 102.2.4 Bilan des mthodes de localisation. . . . . . . . . . . . . 10

2.3 Odomtrie visuelle pour la navigation de robots. . . . . . . . . . 112.3.1 Localisation par triangulation. . . . . . . . . . . . . . . 122.3.2 La gomtrie pipolaire. . . . . . . . . . . . . . . . . . 132.3.3 Lajustement de faisceaux. . . . . . . . . . . . . . . . . 152.3.4 Le filtrage de Kalman tendu. . . . . . . . . . . . . . . . 152.3.5 Le filtre dinformation . . . . . . . . . . . . . . . . . . . 172.3.6 Le filtre particules . . . . . . . . . . . . . . . . . . . . 182.3.7 Bilan des mthodes dodomtrie visuelle. . . . . . . . . 19

2.4 Odomtrie visuelle par filtrage de Kalman tendu. . . . . . . . . 192.4.1 Observation de points en coordonnes cartsiennes. . . . 212.4.2 Observation de points en coordonnes sphriques. . . . . 252.4.3 Conclusion. . . . . . . . . . . . . . . . . . . . . . . . . 27

2.5 Utilisation de donnes Lidar pour laide la localisation . . . . . 28

vi Table des matires

2.5.1 Introduction. . . . . . . . . . . . . . . . . . . . . . . . . 282.5.2 Odomtrie visuelle par couplage Lidar/Camra. . . . . . 30

2.5.2.1 Principe . . . . . . . . . . . . . . . . . . . . . 302.5.2.2 Ajout des donnes Lidar dans le vecteur dtat. 312.5.2.3 Asynchronisme entre les mesures Lidar et les

mesures de Localisation. . . . . . . . . . . . . 322.5.2.4 Slection des points Lidar dans le vecteur dtat33

2.5.3 Mise en correspondance de points issus du tlmtre. . . 342.5.4 Bilan de la fusion Camra-Tlmtre. . . . . . . . . . . 35

2.6 Perception Intelligente de lenvironnement dans lespace de la camra352.6.1 Introduction. . . . . . . . . . . . . . . . . . . . . . . . . 352.6.2 Mthode classique. . . . . . . . . . . . . . . . . . . . . 36

2.6.2.1 Principe . . . . . . . . . . . . . . . . . . . . . 362.6.2.2 Recherche des appariements dans lespace camra372.6.2.3 Cot opratoire de la mthode. . . . . . . . . . 37

2.6.3 Mthode point par point. . . . . . . . . . . . . . . . . . 402.6.3.1 Principe . . . . . . . . . . . . . . . . . . . . . 402.6.3.2 Recherche des appariements dans lespace camra402.6.3.3 Cot opratoire de la mthode. . . . . . . . . . 41

2.6.4 Bilan de la perception focalise. . . . . . . . . . . . . . 422.7 Correction de la drive de la localisation absolue. . . . . . . . . 43

2.7.1 tat du systme. . . . . . . . . . . . . . . . . . . . . . . 472.7.2 Estimation du biais~B + ~Dt . . . . . . . . . . . . . . . . 48

2.7.2.1 volution du biais. . . . . . . . . . . . . . . . 482.7.2.2 volution par addition. . . . . . . . . . . . . . 522.7.2.3 Mise jour du biais. . . . . . . . . . . . . . . 552.7.2.4 Prise en compte du biais et correction de ltat

estim . . . . . . . . . . . . . . . . . . . . . . 562.8 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57

3 Modle Numrique de Terrain 593.1 Introduction. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 593.2 Approches de reconstruction de terrain. . . . . . . . . . . . . . . 61

3.2.1 Introduction. . . . . . . . . . . . . . . . . . . . . . . . . 613.2.2 Reconstruction de lenvironnement par Stro-Vision . . . 613.2.3 Reconstruction de lenvironnement par tlmtre 3D dense 643.2.4 Reconstruction de lenvironnement par SLAM. . . . . . 653.2.5 Bilan des mthodes de reconstruction de lenvironnement 65

3.3 Approche de modle numrique de terrain par couplage camratlmtre . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66

Table des matires vii

3.3.1 Positionnement Espace-Temps dune donne Lidar dansleModle Numrique de Terrain. . . . . . . . . . . . . . . 67

3.3.2 Mise jour du MNT lors dune correction de la drive delocalisation . . . . . . . . . . . . . . . . . . . . . . . . . 67

3.4 Stratgie de gestion de la carte de lenvironnement. . . . . . . . 67

3.4.1 Interaction du processus de guidage pour les besoins deperception. . . . . . . . . . . . . . . . . . . . . . . . . . 68

3.5 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71

4 Rsultats exprimentaux 734.1 Rsultats thoriques sur MATLAB. . . . . . . . . . . . . . . . . 73

4.1.1 Procdure de test. . . . . . . . . . . . . . . . . . . . . . 73

4.1.2 Test des diffrentes mthodes de positionnement. . . . . 76

4.1.3 Test des mthodes globales et point par point. . . . . . . 79

4.1.4 Test des diffrentes mthodes dobservation. . . . . . . . 82

4.2 Rsultats en environnement simul. . . . . . . . . . . . . . . . . 85

4.2.1 Rsultat de localisation. . . . . . . . . . . . . . . . . . . 85

4.2.1.1 Introduction du simulateur ASROCAM. . . . . 88

4.2.1.2 Rsultats sans correction de la drive. . . . . . 90

4.2.1.3 Rsultats avec correction de la drive. . . . . . 95

4.2.1.4 Bilan intermdiaire. . . . . . . . . . . . . . . 96

4.2.2 Rsultat de construction du MNT global. . . . . . . . . . 105

4.2.3 Rsultat de construction du MNT local. . . . . . . . . . 107

4.2.3.1 Reconstruction par Lidar seul. . . . . . . . . . 107

4.2.3.2 Reconstruction densifie par la camra. . . . . 108

4.2.4 Bilan . . . . . . . . . . . . . . . . . . . . . . . . . . . . 108

4.3 Rsultats en environnement rel. . . . . . . . . . . . . . . . . . 114

4.3.1 Rsultats de localisation. . . . . . . . . . . . . . . . . . 114

4.3.1.1 Dispositif exprimental. . . . . . . . . . . . . 114

4.3.1.2 Rsultats sans correction de la drive. . . . . . 115

4.3.1.3 Rsultats avec correction de la drive. . . . . . 120

4.3.2 Rsultat de construction du MNT local. . . . . . . . . . 120

4.3.3 Bilan . . . . . . . . . . . . . . . . . . . . . . . . . . . . 125

4.4 Rsultat de temps de Calcul. . . . . . . . . . . . . . . . . . . . . 131

4.4.1 Temps de calcul de la mise jour de lEKF. . . . . . . . 131

4.4.2 Temps de calcul du processus entier en exprimentation . 131

Conclusion - Perspective 135

viii Table des matires

A Annexe 139A.1 Calcul des jacobiennes des observations. . . . . . . . . . . . . . 139A.2 Calibration du systme Camra-Tlmtre. . . . . . . . . . . . . 144

Bibliographie 147

Table des figures

1 Un vhicule de transport urbain autonome : le VIPA.. . . . . . . xv

1.1 Stanley, le vainqueur du DARPA Grand Challenge 2005. . . . . . 21.2 La dmarche de traitement des informations.. . . . . . . . . . . . 31.3 La comparaison entre une dmarche linaire ( gauche) etune d-

marche cognitive ( droite).. . . . . . . . . . . . . . . . . . . . . 31.4 La voiture autonome du laboratoire Italien VisLab.. . . . . . . . 41.5 La voiture autonome du projet de Google : la Google Car.. . . . . 5

2.1 Les effets de lignorance du facteur dchelle.. . . . . . . . . . . 122.2 La gomtrie pipolaire. Source [Forsyth et Ponce, 2002]. . . . . 132.3 Lajustement de faisceaux local propos par Mouragnon lorsque la

prise de vue de la camraCi est ajoute.. . . . . . . . . . . . . . 162.4 Premire faon dobserver un point : coordonne cartsienne . . . 212.5 Observation dun amer dans la camra.. . . . . . . . . . . . . . . 232.6 Deuxime faon dobserver un point : coordonne sphrique . . . 252.7 Observation dangle dans une camra. . . . . . . . . . . . . . . 282.8 Points de Harris extrait de limage vis--vis de la zone dintrt. . 292.9 Le systme camra-tlmtre. . . . . . . . . . . . . . . . . . . . 302.10 Le couplage Lidar-Camra : utiliser les informations du tlmtre

dans lodomtrie visuelle.. . . . . . . . . . . . . . . . . . . . . . 312.11 Le positionnement dans le repre monde des donnes tlmtre. . 322.12 Prise en compte de la dsynchronisation entre donnes camra et

donnes tlmtre.. . . . . . . . . . . . . . . . . . . . . . . . . 332.13 La rcolte des patchs dans une image valide : on projetteles don-

nes Lidar dune datej dans une image camra de date validekpuis on rcupre les patchs de cette image.. . . . . . . . . . . . . 35

2.14 La focalisation dattention : utiliser les informations prcdentespour les nouvelles recherches.. . . . . . . . . . . . . . . . . . . 36

2.15 La focalisation dattention : utiliser la covariance de lobservationpour la recherche dappariement.. . . . . . . . . . . . . . . . . . 37

x Table des figures

2.16 La projection des ellipses dincertitude dans lespace camra enmthode globale.. . . . . . . . . . . . . . . . . . . . . . . . . . 38

2.17 La projection des ellipses dincertitude dans lespace camra enmthode point par point (en vert).. . . . . . . . . . . . . . . . . 41

2.18 Utilisation des matrices creuses en mthode point par point. . . . . 412.19 La courbe dvolution du cot opratoire de chaque mthode.. . . 432.20 Vue de dessus de la trajectoire effectue par le robot virtuel (en

noir) ainsi que la trajectoire estime par lodomtrie visuelle (enbleu). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44

2.21 Erreur de localisation enX,Y et Z de lEKF par rapport unetrajectoire de rfrence et incertitudes associes.. . . . . . . . . . 45

2.22 Le modle dvolution du vhicule.. . . . . . . . . . . . . . . . 482.23 Evolution de lincertitude de position pour une incertitude en cap

de . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 502.24 Ellipse de lincertitude en position en utilisant une jacobienne de

lvolution . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 512.25 Ellipses dincertitudes possibles pour englober la position vraie. . 522.26 Dfinition des incertitudes en fonction du biais en cap.. . . . . . 532.27 Correction de lodomtrie visuelle : transformation rigide du repre

estim au repre absolu. . . . . . . . . . . . . . . . . . . . . . . 56

3.1 tre ou ne pas tre un obstacle.. . . . . . . . . . . . . . . . . . . 603.2 La partie de lenvironnement utile pour le guidage du vhicule. . . 613.3 Une paire stro et le champ de vision commun aux 2 camras. . . 623.4 La rectification de deux images stro.. . . . . . . . . . . . . . . 633.5 Une carte de disparit produite avec une paire stro.. . . . . . . 633.6 Slection de points : les points qui passent derrire le vhicule sont

supprims de la carte.. . . . . . . . . . . . . . . . . . . . . . . . 693.7 Configurations o des espaces non-observables de lenvironnement

apparaissent.. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70

4.1 Linitialisation dun amer dans lEKF "classique" partir dunpoint tlmtre.. . . . . . . . . . . . . . . . . . . . . . . . . . . 74

4.2 Vue de la trajectoire effectue par le robot virtuel.. . . . . . . . . 754.3 Vue de dessus de la trajectoire effectue par le robot virtuel ainsi

que les trajectoires estimes par les diffrents types de localisation(GM : Mthode globale).. . . . . . . . . . . . . . . . . . . . . . 77

4.4 Erreur totale moyenne des 100 ralisations des processus pour lesdiffrents types de localisation.. . . . . . . . . . . . . . . . . . . 78

4.5 Erreur moyenne de localisation des 100 ralisations enX,Y et Zde chaque mthode par rapport la trajectoire de rfrence.. . . . 79

Table des figures xi

4.6 Erreur moyenne dorientation des 100 ralisations en tangage, la-cet, roulis (, et) de chaque mthode par rapport lorientationde rfrence.. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 80

4.7 Erreur moyenne commise sur la mtrique des 100 ralisations dechaque mthode.. . . . . . . . . . . . . . . . . . . . . . . . . . 81

4.8 Erreurs moyenne de localisation des 100 ralisations enX,Y etZen mthode globale et point par point.. . . . . . . . . . . . . . . 82

4.9 Erreurs moyenne dorientation des 100 ralisations en tangage, la-cet et roulis (, et) en mthode globale et point par point.. . . 83

4.10 Erreur totale moyenne des 100 ralisations en mthode globale(GM) et point par point (PPM).. . . . . . . . . . . . . . . . . . . 84

4.11 Erreur totale moyenne des 100 ralisations suivant chaque mthodedobservation.. . . . . . . . . . . . . . . . . . . . . . . . . . . . 85

4.12 Erreurs moyenne de localisation des 100 ralisations enX,Y etZsuivant chaque mthode dobservation.. . . . . . . . . . . . . . . 86

4.13 Erreurs moyenne dorientation des 100 ralisations entangage, la-cet et roulis (, et) suivant chaque mthode dobservation.. . 87

4.14 Vue externe du scnario prsent sur le simulateur.. . . . . . . . 914.15 Erreur totale de localisation en utilisant la mthode globale.. . . . 924.16 Vue du dessus de la trajectoire calcule en mthode globale. . . . 924.17 Erreur de localisation suivant chaque variable en mthode globale. 934.18 Erreur dorientation suivant chaque variable en mthode globale.. 944.19 Erreur totale de localisation en utilisant la mthode point par point. 954.20 Vue du dessus de la trajectoire calcule en mthode point par point. 964.21 Erreur de localisation suivant chaque variable en mthode point par

point. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 974.22 Erreur dorientation suivant chaque variable en mthode point par

point. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 984.23 Vue du dessus de la trajectoire parcourue en environnement simul

aprs correction de la drive.. . . . . . . . . . . . . . . . . . . . 994.24 Erreur de localisation suivant chaque variable aprs correction de

la drive.. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1004.25 Erreur dorientation suivant chaque variable aprs correction de la

drive. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1014.26 Le phnomne de glissement pour lappariement des points dune

image lautre . . . . . . . . . . . . . . . . . . . . . . . . . . . 1024.27 Glissements enu et v obtenus par rapport aux vraies projections

des amers. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1034.28 Erreur totale de localisation en utilisant la mthode point par point

en absence de glissement.. . . . . . . . . . . . . . . . . . . . . . 104

xii Table des figures

4.29 Modle numrique de terrain obtenu partir des points tlm-triques en utilisant la vraie pose du robot.. . . . . . . . . . . . . 106

4.30 Modle numrique de terrain obtenu partir des points tlm-triques en utilisant lobservation cartsienne en mthodepoint parpoint. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 106

4.31 Modle numrique de terrain local obtenu partir des points tl-mtriques seuls pour litration 300.. . . . . . . . . . . . . . . . 109

4.32 Modle numrique de terrain local obtenu partir des points tl-mtriques seuls pour litration 800.. . . . . . . . . . . . . . . . 110

4.33 Erreur de reconstruction locale obtenu sur lensemblede la trajec-toire simul . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 111

4.34 Histogramme des erreurs de reconstruction obtenu sur une itrationdu processus.. . . . . . . . . . . . . . . . . . . . . . . . . . . . 111

4.35 Modle numrique de terrain local obtenu pour litration 300 avecpeu de donnes tlmtriques (une acquisition tout les mtres). . . 112

4.36 Modle numrique de terrain local obtenu pour litration 300, aug-ment avec les donnes camra.. . . . . . . . . . . . . . . . . . 113

4.37 Notre vhicule dexprimentation AROCO lors du SymposiumROBOTICS 2010.. . . . . . . . . . . . . . . . . . . . . . . . . . 114

4.38 Erreur totale de localisation en exprimentation relle sans correc-tion de la drive.. . . . . . . . . . . . . . . . . . . . . . . . . . . 116

4.39 Vue du dessus des trajectoires calcules en exprimentation rellesans correction de la drive.. . . . . . . . . . . . . . . . . . . . . 117

4.40 Paramtres de localisation suivant chaque variable enexprimen-tation relle sans correction de la drive.. . . . . . . . . . . . . . 118

4.41 Paramtres dorientation suivant chaque variable en exprimenta-tion relle sans correction de la drive.. . . . . . . . . . . . . . . 119

4.42 La correspondance entre la variation de ltat et lenvironnementpour des points cls du parcours.. . . . . . . . . . . . . . . . . . 121

4.43 Vue du dessus de la trajectoire parcourue en environnement relleaprs correction de la drive.. . . . . . . . . . . . . . . . . . . . 122

4.44 Erreur de localisation suivant chaque variable aprs correction dela drive.. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 123

4.45 Erreur dorientation suivant chaque variable aprs correction de ladrive. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 124

4.46 Schma des moments cls de lexprimentation o serontmontrsle MNT en local. . . . . . . . . . . . . . . . . . . . . . . . . . . 125

4.47 Modle numrique de terrain local obtenu en environnement relpour litration 180.. . . . . . . . . . . . . . . . . . . . . . . . . 126

4.48 Modle numrique de terrain local obtenu en environnement relpour litration 325 . . . . . . . . . . . . . . . . . . . . . . . . . 127

Table des figures xiii



4.51 Modle numrique de terrain local obtenu en environnement relpour litration 1920. . . . . . . . . . . . . . . . . . . . . . . . . 130

4.52 Temps de calcul de la mise jour de lEKF pour la mthode globaleen fonction du nombre damers dans le vecteur dtat.. . . . . . . 132

4.53 Temps de calcul de la mise jour de lEKF pour la mthode pointpar point en fonction du nombre damers dans le vecteur dtat. . . 132

4.54 Temps de calcul en mthode globale en fonction du nombredamers dans le vecteur dtat.. . . . . . . . . . . . . . . . . . . 133

4.55 Temps de calcul en mthode point par point en fonction dunombredamers dans le vecteur dtat.. . . . . . . . . . . . . . . . . . . 134

4.56 Lutilisation du pos de robot pour prendre en compte leprincipeque le robot roule sur le sol.. . . . . . . . . . . . . . . . . . . . . 137

A.1 La calibration : passer par un repre tiers. . . . . . . . . . . . . . 146

Introduction Gnrale

Ces dernires annes, les vhicules autonomes sont devenusun enjeu cono-mique et cologique de la socit. En effet, que ce soit dans le domaine civil, mi-litaire ou agricole, de nombreuses applications peuvent bnficier des avancesscientifiques et technologiques de la communaut robotique. Par exemple en agri-culture, linflation de la puissance et du poids des machinescause des dommagesaux sols les rendant plus sensibles aux phnomnes de lessivage. De plus leurgrande taille rend difficile leur contrle prcis par un oprateur, ce qui parait peucompatible avec les exigences dune agriculture soucieusedun dveloppement du-rable. Des solutions bases sur des dveloppement robotiques sont proposes. Ellesimpliquent des vhicules de taille moyenne dont un est dirig par un oprateur, lesautres tant guids par un systme automatique.

Cela se produit aussi dans dautres domaines comme les transports en commun.Par exemple les Vhicules Individuels Publiques Autonomes(VIPA) permettent letransport de passagers en milieu urbain sans conducteur (voir figure 1) : cest leconcept de lascenseur horizontal dont la fonction est de transporter des personnessur la fin de leurs parcours en complment des transports en commun actuels.

Pour tre conomiquement viable, ce concept peut avantageusement bnficierde lutilisation de machines autonomes cest dire fonctionnant sans intervention

FIGURE 1 Un vhicule de transport urbain autonome : le VIPA.

xvi Chapitre 0. Introduction Gnrale

humaine. Si lintrt conomique est clairement justifi dans cette approche elledoit galement proposer une scurit dutilisation conforme aux standards atten-dus. On peut dores et dj arguer le fait que lutilisation ou le fonctionnementautonome permettront de saffranchir des problmes de fatigue accumule par lesconducteurs de vhicules ; cette fatigue tant la source depertes de vigilance deces derniers. Dans le domaine militaire, on peut voquer le cas du dminage ou duconvoi de vhicules humanitaires pour lesquels des robots autonomes pourraientexplorer un espace avec peu dintervention de la part des oprateurs (exemple :projet RINAVEC1).

Toutes ces applications exploitent des dveloppement robotiques importantsvisant augmenter lautonomie dcisionnelle des vhicules. Mais pour pouvoirconsidrer un vhicule comme autonome il doit disposer de certaines fonctionnali-ts :

Un module permettant danalyser sa situation actuelle et les actions mettreen place pour satisfaire la demande exprime (mission-objectif).

Un module permettant de rcolter des informations sur son environnementet/ou sa localisation.

Un module permettant de commander ses actions.Clairement, ces trois modules sont en interactions les uns avec les autres. Par

exemple les actions du robot dpendront de ses capacits percevoir son environ-nement. De mme sa stratgie pour connatre sa situation dpendra des capacitsperceptives mises sa disposition.

Dans des applications spcifiques, un robot peut trs bien avoir seulement cer-taines fonctionnalits pour tre considr comme autonome. Citons notamment laligne 14 du mtro Parisien qui transporte chaque jour des milliers de passagers. Sonprocessus dcisionnel est trs fortement rduit du fait de lenvironnement fermdans lequel elle volue. Cependant, toutes ces fonctionnalits doivent tre prsentesdans un milieu naturel tout comme en milieu urbain et leurs interactions possiblessont trs nombreuses.

Si lon considre ces problmatiques indpendamment les unes des autres, plu-sieurs solutions existent. Des solutions technologiques ont t dveloppes pourpercevoir lenvironnement. En effet la reconstruction de terrain est simple en soids lors que lon dispose dun capteur tlmtrique dense. Cependant ces techno-logies ne sont pas suffisantes pour rpondre lensemble descontraintes de per-ception dun vhicule autonome quest la perception, notamment en terme de loca-lisation. De plus, les cots engendrs par ces technologiessont prohibitifs.

Les travaux prsents dans ce mmoire proposent une solution de percep-tion de lenvironnement au sens large du terme (i.e. localisation et gomtrie

1. RINAVEC : Reconnaissance dItinraire et Navigation en convoi de Vhicules Communi-cants

xvii

du monde) dun vhicule voluant en milieu ouvert non instrument. Aucuneconnaissancea priori de cet environnement nest disposition du systme. Lesproblmes du processus dcisionnel et de laction ne serontpas traits dans cemmoire. Cependant pour rpondre aux besoins dun vhiculeautonome, lesinteractions avec ces fonctionnalits seront prises en compte.

Objectifs de la thse

Comme nous lavons vu, percevoir cest la fois connatre lenvironnement parrapport soi-mme, mais aussi se connaitre par rapport lenvironnement. Autre-ment dit cest pouvoir se localiser, la fois relativement lenvironnement prochemais aussi globalement dans le monde. Cest galement savoir modliser lenvi-ronnement pour pouvoir y dceler les informations qui sont pertinentes. Cependantle monde est rempli dinformations riches et complexes, et ces informations sontsouvent de diffrentes natures. Des systmes ont t crs pour capter ces infor-mations, et ceci de plusieurs faons. Ces capteurs sont dits"extroceptifs" pourpercevoir les informations venant de lextrieur ou "proprioceptifs" pour celles quirelatent du comportement interne du robot.

Si on prend ltre humain comme rfrence, on voit tout de suite quil a luiaussi plusieurs "capteurs" pour laider percevoir le monde. Il se sert de ses yeuxpour voir la gomtrie devant lui et pour se localiser en relatif par rapport celui-ci. Il utilise ses mains pour dterminer la nature des objetsainsi que la distancequi les spare. Ses oreilles lui permettent danticiper lesmouvements tout autourde lui. Grce son oreille interne, il peut trouver son orientation rfrence parrapport la gravit. Au final, cet ensemble de "capteurs" luidonne la possibilitdvoluer librement dans son environnement car le processus de dcision/action deson cerveau dispose des informations ncessaires. Aussi, cette perception peut tresollicite pour mieux rpondre aux besoins car la finalit decet ensemble est lanavigation. Cest ce systme cognitif dans sa globalit quifait la force des tresvivants.

Maintenant si on regarde ce qui se fait "classiquement" dansles domaines scien-tifiques, on remarque que cet aspect cognitif nest pas pris en compte. En effet, lesmthodes classiques de guidage autonome suivent une logique squentielle. La plu-part du temps, on commence par percevoir : on utilise alors les algorithmes les plusperformants pour arriver aux meilleurs rsultats de localisation et de reconstructionde lenvironnement. Puis, ces rsultats servent alimenter les systmes dcision-nels qui doivent absorber toutes ces informations pour en extraire le comportementsouhait du vhicule en esprant quil corresponde aux objectifs de lapplication.Finalement ce comportement est obtenu au moyen des lois de commandes et desactionneurs embarqus sur le vhicule. Tout ceci est ritr aussi souvent que faire

xviii Chapitre 0. Introduction Gnrale

se peut. Ce cheminement a permis de mettre en place des robotsmobiles autonomesen situation relle. Cependant la mise en pratique de ce formalisme requiert beau-coup dhypothses de travail. Ces hypothses vont de la limite dure de vitesse denavigation la puissance de calcul ncessaire quels que soient les besoins de lap-plication. Par exemple si le systme de perception requiertbeaucoup de ressourcespour ses calculs, alors les fonctionnalits suivantes sentrouvent pnalises. Uneraction en chane se produit faisant chouer le systme.

Lobjectif de cette thse est de concevoir un systme de perception intelli-gent pour un robot mobile voluant dans un milieu naturel. Onsintresseraparticulirement deux fonctions :

la localisation absolue intgre permettant de garantir que le vhiculereste dans un couloir de navigation et suffisamment prcise pour per-mettre son guidage automatique. On sattachera dans ces travaux montrer les amliorations ncessaires aux techniques actuelles pour as-surer cette intgrit de localisation et obtenir une estimation prcise dela pose du vhicule.

la reconstruction dune partie de lenvironnement du robot correspon-dant la zone dans laquelle il est susceptible dvoluer dans un futurproche. Dans cette partie, laccent sera port sur ladquation entre lesbesoins de lapplication et la richesse de la reconstruction de la zone per-ue localement par le robot puis sa projection dans le repre global denavigation.

Contributions

Les recherches scientifiques de ces dernires annes ont apport beaucoup din-novations dans le domaine de la perception, que ce soit sur les aspects localisationou reconstruction de la gomtrie dune surface. On peut notamment parler des fer-metures de boucles pour les systmes de localisation ou des mthodes hors-lignedajustement de donnes. Dun autre cot, dans les domainesdcisionnels et deguidage les innovations sont elles aussi importantes. Cependant, les innovationsde ces domaines vont chacune de leur ct et un foss commence se creuser.Par consquent mme un systme dit "classique" a du mal tremis en uvre cause des carts scientifiques trop importants entre perception et guidage. De plusen plus souvent des problmes de scurit apparaissent ce qui freine la diffusion deces technologies. Pourtant, chacun sait quil vaut mieux valider chaque tape l-mentaire avant de sattaquer au problme de faon macroscopique. Dans notre cas,nous pensons quil vaut mieux valider notre approche de systme cognitif. Aussi,les travaux mens dans cette thse ne cherchent pas faire dela perception pourla perceptionmais visent implanterune perception pour lapplication. Dans ce

xix

cadre, deux contributions seront suggres :

A Fusion de camra et tlmtre pour une localisation amlioreNous proposons une mthode de fusion de donnes camra plus tlmtre.Ce procd permet dutiliser une connaissancea priori dinformations deprofondeur du laser dans une image camra. Ces travaux sont dcrit dans[Malartreet al., 2009b].

B Stratgie de Calcul pour une localisation et une reconstruction de gom-trie temps relleUne fois la fusion de donnes effectue, la localisation du vhicule est calcu-le en utilisant uniquement le temps de calcul disponible. Ensuite cette donnede positionnement est utilise pour effectuer une reconstruction 3D de lenvi-ronnement grce encore aux informations tlmtre. Tous ces procds ont texposs dans [Malartreet al., 2010].

Organisation du mmoire

Pour rpondre la problmatique de perception intelligente le mmoire va treorganis autour de 4 parties :

Le contexte de travail ainsi que la philosophie de notre approche seront expli-qus dans le chapitre1. Nous partirons de lobjectif atteindre pour exprimer nosbesoins en terme de perception.

Notre mthode de localisation et perception de lenvironnement par fusion dedonnes sera prsente dans le chapitre2. Nous dcrirons comment extraire une lo-calisation qui rponde aux besoins de lobjectif grce lutilisation de nos capteurs.Ensuite nous expliquerons comment nous obtenons une gomtrie du monde dansle chapitre3. Linteraction du systme de perception avec le processus dcisionnelsera explique.

Dans le chapitre4 nous exposerons notre procdure de mise en uvre ainsi queles rsultats obtenus.

Finalement, nous conclurons et donnerons quelques perspectives de ces tra-vaux.

Chapitre 1

Problmatique : la navigationautonome

Faire naviguer un robot de manire autonome est une tche difficile mettreen uvre qui ncessite la mise en oeuvre dun certain nombre de domaines decomptence. Ces comptences vont du plus bas niveau de conception comme lamcanique des actionneurs au concept de plus haut niveau comme la planificationde trajectoires scurises. Elles ont pour objectifs la ralisation de la mission denavigation mais galement la ncessit de prserver lintgrit du robot et celle deson environnement (personne, objet, autre robots, etc...). Pendant de nombreusesannes ce double challenge a souvent t jug comme une utopie par la socit.Pourtant, en 2004, le DARPA1 lance son premier grand challenge dans ce domaine.

Ce challenge consiste parcourir un circuit dans le dsert des Mojaves de plusde 200 km en moins de 10 heures et de faon entirement autonome. Si la premiredition ne sest pas vu attribue de vainqueur, la deuxime en 2005 a t remportepar lquipe de luniversit de Stanford avec son vhiculeStanley(figure1.1).

Ce challenge a permis de montrer des lments de faisabilitdun systme denavigation. Lensemble des techniques utilises par luniversit de Stanford pourraliser le DARPA Grand Challenge a t dtaill dans [Thrunet al., 2006]. Lesnotions essentielles de localisation, de perception de lenvironnement, de calcul detrajectoire ont t mises bout bout afin de raliser ce dfi.

Si les travaux raliss loccasion de ces challenges ont t un formidable ca-talyseur pour le dveloppement et la promotion de la robotique, ils ne prtendentpas dboucher sur des systmes rpondant aux proccupations de la socit. Pourcela, ils doivent tre gnraliss en tant capable dadresser un grand nombre descnarios dont les paramtres sont souvent dynamiques. A cetitre, de nombreuxdveloppements sont en cours comme par exemple ceux du projet ANR SafePla-toon dont lobjet est de mettre en situation des vhicules autonomes dans des en-

1. DefenseAdvancedResearchProjectsAgency

2 Chapitre 1. Problmatique : la navigation autonome

FIGURE 1.1 Stanley, le vainqueur du DARPA Grand Challenge 2005

vironnements intgrant toutes les contraintes dune utilisation relle mme les plusimprobables.

Pour cela, de nouvelles approches doivent tre considrescomme celles pro-poses par Kelly ([Urmsonet al., 2008]) o les auteurs focalisent leur perceptionsur les informations pertinentes de lenvironnement afin deraliser au mieux la na-vigation. Nous considrerons pour notre part que le processus de navigation doitpiloter les ressources et non linverse. En effet, il est plus pertinent de solliciterles ressources au bon moment au regard de lobjectif vis. Ces approches peuventsouvent tre qualifies de cognitives car le fonctionnementdu systme sadapte la dynamique de lenvironnement jusque dans la gestion de ses ressources. Ladiffrence avec les approches classiques est montre en figure 1.3.

1.1 Cahier des charges

Lambition de notre quipe de recherche est de concevoir un systme autonomede navigation qui garantira lintgrit physique du vhicule ainsi que celle de sesoccupants ou des lments extrieurs. Le DARPA grand challenge a montr le ca-ractre raliste de cette ambition. Maintenant, lenjeu des recherches actuelles estde pouvoir gnraliser ce concept diffrents types de vhicules et de pouvoir lesappliquer en rpondant aux problmes de notre socit aussibien sur le plan tech-nique quconomique.

Pour cela il nous apparait vident que la priorit pour notresystme de naviga-tion est la garantie de lintgrit physique du vhicule, des lments environnants etde ses occupants. Aussi nous dcomposerons la notion de guidage autonome selonles tapes suivantes :

A partir dune mission qui lui a t assigne, par exemple aller de sa po-sition actuelle A une position future B, le robot doit dfinir les actions

1.1. Cahier des charges 3

FIGURE 1.2 La dmarche de traitement des informations.

FIGURE 1.3 La comparaison entre une dmarche linaire ( gauche) et une d-marche cognitive ( droite).

excuter pour raliser cette mission. Une approche classique consiste lancer une tche de localisation avant de commander le robot. Un pro-cessus parallle serait alors charg de surveiller lenvironnement prochedu vhicule afin dviter toute collision. Nous proposons une autre vi-sion des choses : pour nous le vhicule doit identifier chaque instantde la mission les meilleures actions mettre en place pour assurer quetoutes les contraintes de sa mission soient respectes et enparticulier unfonctionnement prcis et intgre.

Parmi tous les paramtres dune mission de navigation autonome, la loca-lisation du robot en est certainement un des plus importants. Cependant, latche de localisation nest utile que si le systme de guidage ne satisfait pas


FIGURE 1.4 La voiture autonome du laboratoire Italien VisLab.

aux contraintes de localisation de la mission (voir [Tessier, 2007]). Ainsi lescontraintes de localisation seront probablement trs diffrentes pour une mis-sion qui se droule dans un dsert ou pour une mission effectue dans unbtiment. Cest la stratgie voque prcdemment qui organisera la gestiondes ressources en fonction des paramtres de lobjectif et de ltat du robot.

Cette localisation est gnralement associe a une carte qui peut tre donne lavance ou construite au fur et mesure du droulement de la mission. Laussi, cest la stratgie qui met contribution les lments de la carte ou quila renseigne de faon pertinente.

La prservation de lintgrit du vhicule est fonction des capacits du vhi-cule et des paramtres de lenvironnement quil doit traverser. Ceux-ci sontgnralement estims par des techniques de perception de lenvironnementsouvent coteuses en temps de calcul mais indispensables la ralisationdun systme oprationnel en environnement ouvert. La gestion de cettetape est donc un lment critique pour la stratgie de navigation puisquim-pliquant directement la suret de fonctionnement du vhicule et la mobilisa-tion des ressources.

Aujourdhui plusieurs quipes sont impliques dans lapplication de cesconcepts la conduite automatique de vhicules dans un environnement rural,urbain ou pri-urbain. On citera plus particulirement lquipe Italienne du la-boratoire VisLab de Parme avec leur projet de traverser deuxcontinentssans apriori sur lenvironnement et sans hypothse de prsence oudabsence dautresvhicules(voir figure1.4). Leur projet consistait rejoindre lexposition mondialede Chine en parcourant les 13000 km qui sparent leur laboratoire de Shanghai(http ://viac.vislab.it/).

Un autre projet encore plus mdiatis est celui de lentreprise Google avec ses

1.2. La perception pour laction 5

FIGURE 1.5 La voiture autonome du projet de Google : la Google Car.

vhicules (voir figure1.5) qui ont dj parcouru plus de 2500 km sans laide deconducteurs.

Tous ces projets permettent aujourdhui de concrtiser uneambition scienti-fique qui date du dbut des annes 90 ([Dickmannset al., 1993]) en proposant lespremiers systmes intgrs capables de remplir simultanment les principales fonc-tions du cahier des charges dun systme de conduite automatique.

De ces projets on peut dgager deux grands processus qui sonten interactionpermanente : un premier typsensorieldont lobjet est la comprhension de len-vironnement du robot, un second typaction dont lobjet est de raliser la missionen toute scurit.

Le fonctionnement des processus sera optimal sils sont aliments par une stra-tgie intgrant en plus de lobjectif de la mission leurs interactions. Dans notre cason parlera deperception pour laction.

1.2 La perception pour laction

Linteraction entre les processus de perception et daction se traduit par unerelation bi-directionnelle :

Linfluence de la tche de perception sur la tche action en terme de prcisionet dintgrit des rsultats fournis ainsi quen terme de ressources consom-mes.

Linfluence de la tche action sur la tche perception en terme de besoinsexprims, de prdiction et dinfluence sur les conditions dexcution du pro-cessus de perception.


1.2.1 Linfluence de la perception sur le guidage

Le systme de guidage est contraint dans ses dplacements par les limites dela tche de perception. Ses actions sont excutes conditionnellement sa connais-sance de lenvironnement. Par exemple la vitesse maximale que le vhicule pourraatteindre sera limite par la distance de perception. En effet le systme doit se lais-ser le temps (et donc la distance) ncessaire pour stopper levhicule lorsquunlment totalement infranchissable apparait dans le systme perceptif.

Aussi, dans un virage rapide ou lors dun franchissement dun "obstacle", unchangement rapide de trajectoire peut crer temporairement des pertes de localisa-tion et des absences de perception (tout comme chez ltre humain). Ceci amnedes limites de perception en terme de vitesse pour le processus de guidage.

1.2.2 Linfluence du guidage sur la perception

Dun autre ct toute les informations dun point de vue guidage peuvent aussiintervenir sur la tche de perception. Cest dire lorsque le processus de guidagesait quil va tourner il peut trs bien "prvenir" la perception pour ne pas quelle seperde. Il se passe implicitement la mme chose chez ltre humain quand il prendun virage en voiture il regarde du ct de celui-ci. Ensuite,un systme de percep-tion ne sait pas en lui-mme les endroits les plus pertinents regarder. Cependant lastratgie de navigation sait o elle doit aller. Ces informations pertinentes doiventdonc tre "demandes" la perception par le processus de guidage.

Aussi, le systme de navigation sait dterminer linfluencede lenvironnementsur son tat. Par exemple il peut valuer sa position et son orientation lorsque len-vironnement est connu (par un pos de robot par exemple). Il est donc capabledestimer la variation de son tat en fonction de la trajectoire prvue. Cette infor-mation peut tre utilise par le systme de perception pour focaliser ses calculs surun espace possible de rsultat plus restreint.

1.3 Conclusion

Dans ce chapitre nous avons expos lapproche que nous souhaitons mettre enuvre pour raliser notre systme de navigation. Ce systmedoit invitablementcomporter un systme de perception et un systme de navigation. Cependant notrephilosophie diffre dans le sens o perception et action ne sexcutent pas de fa-on squentielle mais interagissent lune avec lautre et partagent des informationsde faon bi-latrale afin de raliser au mieux la tche demande dans un soucisconstant de scurit.

Chapitre 2

Localisation dun vhicule par fusionCamra-Tlmtre

2.1 Introduction Gnrale

Lorsque lon parle de navigation autonome, les systmes GPSsont trs souventcits. Cette addiction au systme GPS vient du fait que lon associe implicitementlautonomie dun vhicule avec un systme de localisation.En effet, on imaginemal rejoindre notre destination si lon ne sait pas o lon est. Cette donne de loca-lisation constitue le retour dinformation de notre systme de guidage. Sans cela,aucun systme ne peut converger vers une solution proche de la valeur finale. Cettelocalisation est de ce fait trs importante si on veut se rapprocher dun systmecompltement autonome. La priorit de notre systme de perception est de recons-truire le monde lavant du robot. Cette reconstruction dpend de la localisationdu vhicule qui est value au fur et mesure de lavance du robot. La questionquil faut se poser maintenant est "Comment va-t-on se localiser ?".

Ce chapitre est ddi la localisation dun vhicule dans son environnementmais aussi dans le monde puisque comme nous lavons vu ces deux aspects nepeuvent tre dissocis. Lobjectif de cette partie est de mettre en place un systmede localisation la fois prcis localement et globalement,qui permettra la mise enplace de la reconstruction de lenvironnement.

2.2 Techniques de localisation pour vhicules auto-nomes

Pour se localiser, nous avons besoin dinformations relatives ltat de notrevhicule. La localisation dun vhicule ou dun objet dpend du repre initial danslequel elle va tre effectue. On peut mettre en vis--vis deux grands principes : la

8 Chapitre 2. Localisation dun vhicule par fusion Camra-Tlmtre

localisation absolue et la localisation relative.

2.2.1 Localisation absolue

Le principe de la localisation absolue est davoir accs auxparamtres qui d-finissent la position du robot dans lunivers. Dun point de vue pratique, on peutlimiter cette connaissance la position sur terre. Le GPS (Global Positioning Sys-tem) est un systme de localisation par satellite cr dans les annes 1970 par ledpartement de la dfense des tats-Unis pour calculer la localisation dun rcep-teur sur terre dans un contexte militaire. Le principe du GPSest dutiliser la mesuredu temps de vol des signaux mis par au moins 4 satellites. La position du rcepteurest ensuite calcule par triangulation de ces mesures avec la position de chaque sa-tellite ( dont la position est prcisment connue). Dautres systmes de localisationabsolue sont raliss en utilisant une base de connaissances gorfrences appelSIG qui permet au vhicule de se situer par rapport aux lments de la base quilperoit.

2.2.1.1 Localisation absolue par GPS

De nombreux travaux concernant le suivi de trajectoire et autres processusde guidage utilisent le systme GPS-RTK comme donne de localisation (voir[Lenainet al., 2006] o les auteurs mettent en uvre un suivi de trajectoire en pr-sence de glissement). Cependant, mme si les donnes fournies par un tel systmeparaissent trs prcises (de lordre du centimtre), ellessont insuffisantes pour plu-sieurs raisons :

Aussi prcise soit cette localisation dun point de vue global, elle nest pasforcment suffisante dun point de vue local. Ces quelques centimtres deprcision peuvent tre trs importants relativement au vhicule et la tchequil doit accomplir.

Ces informations sont dpendantes de lenvironnement. Silon se trouvedans une ruelle troite ou proche dun arbre, des phnomnesde multi-trajetsou mme une perte totale du signal peuvent apparaitre.

Enfin, les informations GPS sont incompltes. La longitude, la latitude sontles donnes principales du GPS, le cap tant quant lui calcul avec unesuccession de mesures. En revanche, les informations de roulis et de tangagedu vhicule ne sont tout simplement pas prsentes alors quun systme deguidage pourrait en avoir besoin pour assurer son intgritphysique.

2.2.1.2 localisation absolue par SIG

On peut aussi raliser une localisation absolue en utilisant un SIG (SystmedInformations Gographique). Un SIG est une base de donnes damers go-

2.2. Techniques de localisation pour vhicules autonomes 9

rfrencs. En observant ces amers travers un capteur extroceptif (comme unecamra par exemple dans [Jensfelt et Kristensen, 2001]) et en utilisant des m-thodes de triangulation nous pouvons remonter une localisation absolue de notrerobot. Dans notre application, les interactions avec lenvironnement se font locale-ment. Ces mthodes sont en gnral trs prcises mais dpendent fortement de laqualit de la carte sur laquelle elles sappuient. Ainsi, des lments non cartogra-phis mais nanmoins dtects par les capteurs du vhicule peuvent tre apparisavec des lments prsents de la base de donnes ce qui va rendre le systme delocalisation non-intgre. Ils peuvent cependant tre avantageusement utiliss parle systme comme cela est propos par les mthodes localisation et cartographiesimultan (SLAM).

Si le rfrencement absolu de la localisation dun vhiculea un intrt ind-niable dans la gestion des objectifs de sa mission (points depassage, couloir denavigation, ...) il comporte des limites dues labsence dinformation locale sus-ceptible de changements incompatibles avec une gestion globale de la navigation.

2.2.2 Localisation relative

Lautre faon de voir la localisation est plus centre sur unaspect local depositionnement. En localisation relative nous cherchons savoir, en partant duneposition initiale connue, de combien sest dplac le vhicule relativement sonancienne position. Lintrt de cette mthode est que linteraction entre localisa-tion et guidage est permanente puisque lapplication va chercher voluer dansun environnement dont les obstacles sont dfinis de faon locale par rapport lui.La mthode la plus simple pour effectuer une localisation relative est dutiliserles informations des capteurs proprioceptifs du vhicule comme les odomtres, lesgyromtres ou encore langle de braquage des roues. La mesure fournie par un odo-mtre est la distance curviligne parcourue par la roue pendant un laps de temps. Legyromtre est un capteur de vitesse angulaire qui peut, par exemple, dterminer lechangement de cap du vhicule. Si on associe ces informations un modle cin-matique de vhicule (voir [Campionet al., 1996]) on peut estimer le dplacementrelatif de notre vhicule. Une fusion plus pousse de ces capteurs permet dobte-nir des informations plus prcises encore comme dans [Borenstein et Feng, 1996]par exemple o les auteurs proposent de fusionner lodomtre et le gyromtre pourobtenir une information plus pertinente de lorientation du vhicule.

Une autre faon est de recourir une centrale inertielle quifournit lesparamtres dacclration linaire et de vitesse angulaire dun objet (voir[Barbour et Schmidt, 2001]). Ensuite, une double intgration de ces informationspermet de remonter une localisation 6D du robot.

Nous pouvons aussi nous localiser en relatif en utilisant des capteurs extrocep-tifs comme un tlmtre laser ou une camra par exemple. Pourcela, une observa-


tion dun amer fixe dans lespace capteur plusieurs fois de suite permet de retrouverle dplacement relatif du robot. Le SLAM (Simultaneous Localization And Map-ping) est une technique de localisation et cartographie quisinspire de ce principe(voir [Durrant-Whyte et Bailey, 2006] et [Bailey et Durrant-Whyte, 2006]).

Le problme principal des mthodes de localisation relative est quelle su-bissent des drives plus ou moins grandes au cours du temps. De plus, mme sion connait le dplacement relatif de notre robot partir dun repre initial, on neconnait pas le positionnement initial du vhicule dans un repre monde. Le pro-blme est que les lois de commande utilises pour actionner le vhicule sont, laplupart du temps, crites pour fonctionner dans un rfrentiel absolu. Une solutionde localisation relative nest donc pas suffisante.

2.2.3 Fusion de localisation absolue-relative

Nous avons vu que la localisation est une tape trs importante pour prtendre une autonomie totale des robots. Cette localisation doit tre plus o moins com-plte en terme dinformation suivant le contexte de travail. Par exemple, dansses travaux Laneurit cherche calculer la position de son vhicule sur une route[Laneurit, 2006]. Pour cela il estime la position et le cap de son vhicule en consi-drant una priori de sa position sur un monde plan.

La combinaison de localisation relative et absolue est possible comme dans[Btailleet al., 2007] o les auteurs fusionnent plusieurs types de capteurs ext-roceptifs et proprioceptifs pour arriver un rsultat de localisation trs prcis etprsentant peu de drives. Cependant la mise en uvre de cette fusion de capteursest lourde et rsout seulement le problme de localisation et pas celui de la percep-tion de lenvironnement. De plus, pour arriver une telle prcision, les dispositifsutiliss sont trs coteux.

Les travaux de Tessier [Tessier, 2007] comportent les mme problmatiquesque prcdemment avec pour objectif supplmentaire de guider le vhicule et non-plus de le localiser uniquement. Le but de ses travaux est de guider un vhiculesur une trajectoire connue (rfrence dans un repre monde) et ralisable (la tra-jectoire ne comporte pas dobstacles contourner). Cette approche est nommele SLAG (Simultaneous Localization And Guidance). Elle permet dassurer uneprcision sur le positionnement du robot laquelle est associe une confiance enutilisant les mmes hypothses de monde plat.

2.2.4 Bilan des mthodes de localisation

Dans notre contexte de travail, pour lequel le vhicule volue dans un environ-nement naturel, cette hypothse de monde plan ne peut pas tre utilise. En effet

2.3. Odomtrie visuelle pour la navigation de robots 11

on ne peut pas assurer une intgrit physique si notre robot est proche dun ren-versement latral par exemple. Cette localisation doit donc comprendre toutes lescoordonnes de positionnement dun vhicule, savoir les 3paramtres de posi-tion nots

(

Xv Yv Zv)

et les 3 paramtres dorientation (Tangage, Cap, Roulis)nots

(

v v v)

. Pour cela, les approches de localisation absolue ne sont passuffisantes car :

dans le cas du GPS, elles ne fournissent pas assez de donnesde position-nement et ncessitent des conditions extrieures dutilisation qui ne sont pasapplicables en environnement naturel (possibilit de se trouver dans une fortetc...)

il nexiste pas encore de SIG utilisable assez dtaill pour raliser cette tchede localisation.

Les mthodes de localisation relatives ne sont pas utilisables non plus en ltatpuisque le systme de guidage du vhicule besoin dun positionnement absolu. Deplus les phnomnes de drive prsents dans les techniques de localisation relativerduisent les chances de succs dune navigation autonome.

Finalement cest la fusion de ces deux techniques de localisation qui parait lamieux adapte notre objectif. Cependant, dans notre cas, lhypothse de mondeplat ne peut pas tre applique. Il nous faut dfinir un nouveau systme de locali-sation pour une navigation qui mette en uvre ces deux types de localisation avectoutes les composantes ncessaires. Nous proposons donc destimer le dplace-ment relatif du vhicule par un moyen dodomtrie visuelle tout en la fusionnantavec une localisation absolue par GPS.

2.3 Odomtrie visuelle pour la navigation de robots

Si on se rfre ltre humain, notre capacit nous localiser au sein de notreenvironnement est principalement base sur nos yeux. Cet organe a de plus undouble effet puisquil permet aussi de percevoir la gomtrie du monde dans notrechamp de vision. Cependant le principal inconvnient de lil humain est sa dif-ficult dterminer le facteur dchelle de lenvironnement quil peroit (incon-vnient qui est dtourn par lutilisation de nos deux yeux). Le facteur dchelle(aussi appel la "mtrique") est le facteur qui va nous permettre de transformer unedistance connue sans unit en une distance en mtre. Cette mtrique est trs impor-tante puisquelle nous permet de comparer les dimensions denotre environnementavec celles de notre propre corps (figure2.1). Malgr ce manque de mtrique, lilest tout fait capable de fournir un dplacement relatif prcis. Le pendant lectro-nique de lil humain est la camra projective. De nombreux travaux traitent delocalisation grce des camras. Cependant la dtermination de la mtrique resteun problme dans ce genre dapplication.


Hauteur ?

FIGURE 2.1 Les effets de lignorance du facteur dchelle.

En cartant les problmes de mtrique, les approches basessur des techniquesde vision sont intressantes car elles permettent dobtenir des rsultats complets entermes de localisation et destimation de la gomtrie de lenvironnement avec unebonne prcision : ce sont les techniques dodomtrie visuelle.

Les images successives dune camra sont les principales informations en odo-mtrie visuelle. Ces images une fois traites, servent calculer le dplacement dela camra entre chaque prise de vue. Pour cela, on cherche descorrespondances depoints dans plusieurs images conscutives. Ces correspondances de points consti-tuent lentre du systme qui va permettre dobtenir le dplacement relatif de lacamra de la premire image vers la deuxime. Ce dplacementpeut alors tre cal-cul de diffrentes faons comme nous allons le voir dans lesparagraphes suivants.Il faut garder galement lesprit que lon doit avoir la possibilit destimer lespoints 3D utiliss comme amers et de les prciser au fur et mesure de lavancedu vhicule.

2.3.1 Localisation par triangulation

Dans un cadre idal, faisons lhypothse que les coordonnes 3D des pointsde la carte sont parfaitement connues. Quelle est la mthodepour obtenir la posequi donnerait la projection telle quelle est vue dans la camra ? Le problme estune triangulation de position/orientation. Un groupe deN correspondances 2D/3Dest alors ncessaire pour estimer la pose de la camra. Le nombre N varie enfonction de la mthode utilise et des paramtres estimer (il faut au minimum3 appariements 2D/3D). Haralick [Haralicket al., 1994] a rpertori un ensemble


de mthodes pour estimer la pose avecN = 3 points, Quan [Quan et Lan, 1999]propose deux algorithmes pour respectivementN = 4 etN = 5 points. Bujnak[Bujnaket al., 2008] quant lui propose une mthode pour dterminer la pose etla focale avec 4 points. A ce calcul analytique est souvent associ un processus detirage alatoire RANSAC (voir [Fischler et Bolles, 1981] o les auteurs se serventnotamment de ce processus pour estimer la pose dune camra avec 3 points) quiva permettre de choisir le groupe de points qui minimise les erreurs de projectionsde tous les points apparis. Ces mthodes offrent toutes uneprcision trs bonnede localisation, cependant leur utilisation ncessite la connaissance des points r-frencs dans un repre 3D. Cette connaissance pralable des points 3D est sou-vent obtenue par un calcul hors ligne de reconstruction comme propos par Royer[Royeret al., 2006]. La prcision de la localisation va donc dpendre de la prci-sion de ces points 3D. De plus, dans notre cas, cette hypothse de connaissance despoints 3D nest pas vrifie puisquelle fait aussi partie des objectifs.

2.3.2 La gomtrie pipolaire

La gomtrie pipolaire est la gomtrie qui dfinit les contraintes qui lientplusieurs vues dune mme scne. Soit~P un point 3D quelconque visible dans deuximages de prises de vue diffrentes. SoitO1 etO2 les centres optiques de ces deux

vues. Soit~p1 =

u1v11

et ~p2 =

u2v21

les projections du point~P en coordonnes

homognes respectivement dans la premire et deuxime vue.La figure2.2montrecette configuration.

La projection dun point quelconque~P dans limage dune camra appartientau plan form par les deux centres optiquesO1 etO2 et le point~P .

FIGURE 2.2 La gomtrie pipolaire. Source [Forsyth et Ponce, 2002].

Il en dcoule que la projection~p1 de la premire vue appartient au plan formpar la projection~p2 de la deuxime vue avec les deux centres optiquesO1 etO2.Cette contrainte sappelle lacontrainte pipolaire. Elle est reprsente par une ma-


trice (3x3) appelematrice essentielle(note) et a t introduite pour la pre-mire fois par [Longuet-Higgins, 1981]. Cette matrice contient les informations detranslations et de rotations de la premire camra vers la deuxime un facteurdchelle prs. Elle est construite partir de la matrice derotationR et du vecteurde translation~T =

(

T1 T2 T3)T

de la premire camra vers la deuxime. Plusparticulirement, lequation2.1dfinit cette matrice essentielle.

= [T ].R o [T ] =

0 T3 T2T3 0 T1T2 T1 0

(2.1)

On a donc la relation~pT2 ..~p1 = 0 si la contrainte pipolaire est respecte (onnoteT la transpose dun vecteur ou dune matrice). Dans le cas o les paramtresintrinsques des camras ne sont pas connus, une matrice analogue la matriceessentielle existe : la matrice fondamentale (noteF). La relation qui lie la matriceessentielle la matrice fondamentale est la suivante :

F = KT1 ..K12

o Kj est la matrice des paramtres intrinsques de la camraj. Plusieursmthodes existent pour calculer soit la matrice essentielle soit la matrice fon-damentale. On peut citer notamment la mthode des 8 points introduite par[Longuet-Higgins, 1981] puis revisite par [Hartley, 1995] pour calculer la ma-trice fondamentale. Pour la matrice essentielle, 5 points suffisent pour dtermi-ner tous les paramtres (un point par paramtre de position-orientation moinsun du fait de la mtrique). Des mthodes pour calculer la matrice essentielle etpour en extraire les paramtres de rotations et translations ont t proposes par[Nistr, 2004, Stewniuset al., 2006, Li et Hartley, 2006].

Cette faon de calculer le dplacement dune camra offre debons rsultatsmais ne rsout pas le problme de la mtrique. En effet, la contrainte pipolaireest respecte mme si un facteur quelconque est appliqu au vecteur de translation~T . Ce vecteur est le plus souvent obtenu avec une norme unitaire. Cette mtriquepeut cependant tre chiffre en utilisant des informationsprovenant de lodom-trie des roues par exemple. Le problme est que ce raisonnement donne des rsul-tats approximatifs en environnement naturel puisque lodomtrie traduit la distancecurviligne parcourue par la roue alors que nous avons besoinde la norme de la dis-tance entre les deux positions de la camra. Le relief du sol ou bien encore lesphnomnes de glissement peuvent alors trs facilement induire des erreurs. Aussi,lestimation des coordonnes des points 3D ayant servi comme amers doit se fairepar une tape supplmentaire.


2.3.3 Lajustement de faisceaux

La connaissance des points en 3D est ncessaire si on veut utiliser des m-thodes par triangulation. Cependant dans notre cas nous recherchons aussi cetteinformation puisque nous considrons lenvironnement inconnu. Le problme de-vient le suivant : comment estimer en mme temps la pose de la camra ainsique les positions 3D de points dont les projections dans plusieurs images sontconnues ? Le problme revient alors trouver les poses~Ci et les points 3D~Pj quiminimisent les erreurs de reprojectionsmin(f(C1...Ci, ~P1... ~Pi)). Ce principe estcommunment appel lajustement de faisceaux. Lajustement de faisceaux est unproblme de minimisation globale qui ncessite la connaissance de toutes les infor-mations disponibles au moment de sa rsolution. Ce problmede minimisation estsouvent rsolu par des algorithmes de Levenberg-Marquardt[Presset al., 1992].[Royeret al., 2006] utilise cette mthode pour connaitre lensemble des posesdune camra et des points 3D dun ensemble de donnes pour ensuite les utili-ser comme rfrence dans un guidage de vhicule autonome. Linconvnient ma-jeur de cette approche est quelle ncessite de connaitre toutes les donnes aumoment du calcul. Cela implique davoir effectuer la minimisation sur une trsgrande quantit de donnes. [Mouragnonet al., 2006] a cependant amlior cetteprocdure en effectuanten ligneun ajustement de faisceaux local qui utilise lesdonnes des dernires images pour le calcul des poses et des points (voir figure2.3) une frquence de calcul denviron 7 images par seconde. Les auteurs de[Konolige et Agrawal, 2008] ont eux aussi propos une mthode pour effectuer unajustement de faisceaux en temps rel en limitant grandement le nombre de va-riables (tats du vhicule et amers) estimer.

Cependant, mme si ce procd est compatible avec notre objectif, il a besoindes donnes disponibles dun certain nombre ditrations prcdentes. Ces infor-mations disponibles doivent tre tries en terme dimages cls ou non. Lajustementde faisceaux est alors ralis seulement lorsquune nouvelle image cl est produite.Les coordonnes des points 3D sont alors affines seulement chaque image cl.Un calcul de pose par triangulation est effectu pour les autres images. Malgrle fait deffectuer lajustement de faisceaux seulement pour les images cls, lesperformances dun tel algorithme sont la limite du fonctionnement temps rellors dun rejeu et ne prsagent pas dun fonctionnement correct lors dune expri-mentation en direct sur vhicule dexprimentation (Par exemple lalgorithme deMouragnon fonctionne environ 7 images par seconde).

2.3.4 Le filtrage de Kalman tendu

Comme nous lavons vu, utiliser le systme perceptif pour calculer la pose duvhicule ainsi que la gomtrie de lenvironnement est une tche importante, ce-


FIGURE 2.3 Lajustement de faisceaux local propos par Mouragnonlorsque laprise de vue de la camraCi est ajoute. Seuls les points et les camras encadrssont optimiss. Source : [Mouragnonet al., 2006]

pendant elle est souvent coteuse en temps de calcul. Elle doit tre ralise tout enpermettant au systme de guidage davoir suffisamment de ressource pour contr-ler le vhicule. Actuellement, lapproche par ajustement de faisceaux ne permetpas cette flexibilit dans le calcul puisque tout est rsolu en une passe de la m-thode sans possibilit de contrle sur les ressources ou rsultats obtenir. Il nousfaut donc un moyen destimer la fois la position du vhicule, son orientation maisaussi les positions 3D des points de limage qui nous ont servi damers. De plus, lechoix et la quantit de donnes fournir cet estimateur doit pouvoir tre contr-lable. Le filtre de Kalman est un estimateur qui possde toutes ces particularits.Notre choix se porte plus particulirement vers le filtre de Kalman tendu en raisondu caractre non-linaire de notre application. Le filtre deKalman tendu (EKF)est un estimateur baysien itratif qui concentre lensemble des informations destats prcdents dans son estimation courante. Ltat du filtre est reprsent par

deux informations : ltat estim~X du vrai tat~X et la matrice de covariance decet tat estimP. Ltat vrai du systme volue grce lquation :

~Xk|k1 = f( ~Xk1|k1, ~Uk, ~Qk) (2.2)

(2.3)

o ~Uk est lentre du systme,~Qk est le bruit du processus dvolution,

f(~Xk1|k1, ~Uk, ~Qk) est la fonction dvolution du systme.

Le filtre de Kalman ([Mourllion et al., 2006]) est dcoup en deux tapes ma-jeures :

Prdiction : ltata priori ( linstantk) est estim en utilisant ltat courant


(instantk 1).~Xk|k1 = f(

~Xk1|k1, uk, 0) (2.4)

Pk|k1 = Fk.Pk1|k1.FTk +Qk (2.5)

o Fk est la jacobienne def(~Xk1|k1, ~Uk) et Qk la matrice de covariance

du bruit dvolution du processus. Mise jour : ltata posteriori( linstantk) est calcul en utilisant linno-

vation de lobservation par rapport ltata priori.

yk = h(~Xk|k) (2.6)

Sk = Hk.Pk|k1.HTk +Rk (2.7)

Gk = Pk|k1.HTk .S

1k (2.8)

~Xk|k =

~Xk|k1 +Gk.(yk h( ~Xk|k1)) (2.9)

Pk|k = Pk|k1 Gk.Hk.Pk|k1 (2.10)

o yk est la mesure de ltat,h est la fonction dobservation de ltat,Hksa jacobienne linstantk, Rk la covariance du bruit dobservation etGk legain de Kalman.

Ces dernires annes le filtre de Kalman a t utilis dans beaucoup dappli-cations de diverses natures. Dans notre cas de perception/navigation le filtre deKalman a commenc tre utilis dans un contexte de localisation et cartogra-phie simultane (SLAM : Simultaneous Localization And Mapping en anglais). Parexemple dans [Dissanayakeet al., 2001] les auteurs utilisent ce filtre pour estimerla position et le cap(X, Y, ) dun vhicule dans une carte 2D constitue damers(Axi, Ayi). En ce qui concerne le SLAM par vision, Davison ([Davison, 2003]) at un des tout premiers mettre en uvre une application de ce type fonction-nant en temps rel. Ces approches de type SLAM ont aussi t utilises avec desdonnes provenant de camras stro ([Lemaireet al., 2007]).

Le filtre de Kalman tendu est bien adapt notre problme puisquil permetdestimer la pose courante du vhicule ainsi que les coordonnes des amers aveclancien tat seulement. De plus, il se prte convenablement lexploitation dedonnes proprioceptives et la fusion de donnes extroceptives.

2.3.5 Le filtre dinformation

Il existe dautres filtres se rapprochant du filtre de Kalman.Dans le filtre din-formation par exemple ([Gibbenset al., 2000]), les informations du systme sontcontenues dans le vecteur dinformation~y et la matrice dinformationY. Les re-lations qui lient le vecteur dtat avec le vecteur dinformation et la matrice decovariance avec la matrice dinformation sont les suivantes :


Y = P1

~y = P1.~X (2.11)

Lavantage avec le filtre dinformation est que plusieurs mesures peuvent treprises en compte indpendamment les unes des autres simplement en ajoutant leurseffets au vecteur dinformations et la matrice dinformation. De ce fait, siN me-sures ont t effectues il suffit dajouter cesN informations au vecteur dtat et la matrice dinformation. Notre systme dans son ensemble (perception/action) vadialoguer avec le processus de guidage afin de lui fournir lesinformations dont ila besoin pour voluer. Parmi ces informations transmettrese trouvent les infor-mations de localisation et des amers 3D contenues dans le vecteur dtat et leursincertitudes. Avec ce filtre il nous serait alors ncessairedeffectuer la conversiondu vecteur dinformation et de la matrice dinformation chaque demande ce quincessite dinverser une matrice et donc une consommation des ressources suppl-mentaire.

2.3.6 Le filtre particules

Une autre faon de rsoudre le problme est de tester une multitude dhy-pothses la manire dun tirage alatoire de type Monte-Carlo, cest le filtre particules. Pour cela, on effectue un chantillonnage alatoire de ltat suivant sadensit de probabilit suppose connue. Chacun de ces tirages alatoires consti-tue uneparticule. Un tutoriel sur le filtre particule a t publi par Arulampa-lam [Arulampalamet al., 2001]. Puis, les observations sont utilises pour pondrerchacune de ces particules en terme de probabilit afin de leurdonner plus dim-portance. Si suffisamment de particules sont cres, celle qui possde le plus grosscore est une bonne estimation du vecteur dtat. Cependantcette technique nces-site de faire un tirage alatoire sur lensemble de ltat. Or la position dun amerdans la carte est dpendante de la pose du vhicule. Cest grce cette informationque les algorithmes FastSLAM 1.0 ([Montemerloet al., 2002]) et FastSLAM 2.0([Montemerloet al., 2003]) ont vu le jour. Lide du FastSLAM est de gnrer unchantillonnage sur la trajectoire du vhicule uniquement, les amers tant gn-rs conditionnellement cette trajectoire. Pour chacune de ces trajectoires un filtrede Kalman tendu par amer est appliqu. Linnovation apporte par FastSLAM2 rside dans lamlioration de la qualit des chantillonsgnrs. Cette amlio-ration a donc pour effet de diminuer le nombre de particules ncessaires. Si onmet de ct les problmes dinconsistance du FastSLAM ([Bailey et al., 2006b]),il reste le problme de savoir quelles informations nous allons dlivrer au systmede guidage pour son fonctionnement. Le fait de renvoyer la particule prsentant laplus grosse probabilit peut provoquer des sauts dans lestimation de la trajectoire

2.4. Odomtrie visuelle par filtrage de Kalman tendu 19

puisque lon peut changer de particule tout instant. Il faut aussi gnrer un nombrede particules suffisamment important, chacune de ces particules doit tre vrifie.Aussi, [Baileyet al., 2006b] conclut dans son article que le FastSLAM serait plusappropri linitialisation de la carte sur une courte priode.

2.3.7 Bilan des mthodes dodomtrie visuelle

Nous avons vu que la meilleure approche pour effectuer une localisation parvision serait de faire une triangulation avec des points 3D connus. Cependant nousnavons pas ces informations notre disposition puisque nous avons aussi commeobjectif destimer ces amers 3D. Les techniques de gomtrie pipolaire nont pasbesoin de cette connaissance pour pouvoir tre mises en uvre mais les mmesproblmes destimation des points 3D apparaissent ici aussi. De plus, associer unemtrique correcte au calcul du dplacement est ambige. Lesmthodes dajuste-ment de faisceaux rpondent au problme donn mais ont t cres mon senspour des calculs hors ligne de pose de robots ou de construction de carte. Mme sides mthodes locales dajustement de faisceaux existent, leurs utilisations ne sontpas faites pour des applications temps rel. Pour les filtres particules le problmeest de savoir quelle est linformation de positionnement que nous allons transmettreau processus de guidage. En effet les lois de commande associes au systme deguidage sont faites pour fonctionner avec une localisationuni-modale, le change-ment de modalit peut avoir un effet indsirable sur le systme. Le filtre de Kalmantendu ainsi que le filtre dinformations sont assez procheslun de lautre. Ils pro-posent tout les deux une estimation uni-modale avec estimation de la mtrique. Lesfiltres dinformations ne sont pas adapts un systme complet de perception etguidage puisque linformation de localisation nest pas accessible directement, ilfaut passer du vecteur dinformation au vecteur dtat pourconnaitre la positiondu vhicule et cette tape peut tre trs consommatrice de ressources si beaucoupdinformations sont prsentes dans le vecteur dtat. Finalement, le filtre de Kal-man permet de satisfaire tous les aspects de notre objectif en une seule tape touten permettant une volution en temps rel (cadence de traitement et traitement surflux de donnes direct). Nous utiliserons donc le filtre de Kalman tendu dans lasuite de ce mmoire.

2.4 Odomtrie visuelle par filtrage de Kalmantendu

Afin deffectuer notre odomtrie visuelle, plaons nous dans un cadre destima-tion de pose dune camra monoculaire. Pour mettre en place le filtre de Kalman,nous avons besoin de connaitre les quations dtat et les quations dobservations


de notre systme. Dans notre cas, les observations de ltatcourant du vhiculevont se faire grce la camra. Le vecteur dtat qui constitue la pose du vhiculedans le monde ainsi que les points 3D estimer est donc compos des 6 paramtres

de positionnement (un vecteur not~Xvm) et des coordonnes de chaque point 3Dncessaire (appelamer) notes~Pn :

~Xvm =

(

Xv Yv Zv v v v)T

=

(

~T(vm)~O(vm)

)

~Pn =(

Pxn Pyn Pzn)T

(2.12)

o : ~T(vm) =

(

Xv Yv Zv)T

sont les positions respectivement sur X, Y, Z duvhicule dans le repre monde.

~O(vm) =(

v v v)T

sont les paramtres respectivement de tangage,lacet, roulis du vhicule dans le repre monde par rapport aux axes x, y, z.

~Pn =(

Pxn Pyn Pzn)T

sont les coordonnes de lamern (n [1, N ]) pourrespectivement sa position en X, Y, Z dans le repre monde.

Le vecteur dtat entier de notre systme est alors de la forme :

~X =

(

~T T(vm)~OT(vm)

~P T1 . . .~P Tn . . .

~P TN

)T

oN est le nombre de points prsents dans notre carte.La position estime du vhicule se situe sur le sol au milieu de lessieu arrire.

La camra quant elle se situe lavant du vhicule et en hauteur. Elle est consi-dre fixe par rapport au chssis du vhicule ; de ce fait une transformation rigideexiste entre le repre vhicule et le repre camra. Ce changement de repre estdfini par une rotationR(cv) et une translation~T(cv).

Afin de mettre jour le vecteur dtat de notre systme, une observation decelui-ci doit se faire, ici, au moyen de la camra. Pour cela nous devons donc dfinirsous quelle forme seront observs les points dans la camra.La nature de lobserva-tion a une grande importance dans un EKF. En effet, la jacobienne de ces quationsdobservations doit tre fournie au filtre pour mettre jourltat a posterioriet sacovariance. Cependant le fait dapproximer la fonction dobservation par une fonc-tion linaire (en calculant la jacobienne) amne une non-intgrit des estimationsdans le filtre de Kalman. Une synthse des causes et consquences de linconsis-tance est propose par [Joly, 2010]. Cette non-intgrit est trs pnalisante puisquela vraie position du vhicule nappartient pas lellipse de confiance de ltat.Cette incertitude de localisation peut alors tre fatale auvhicule et ses occupantssuivant le contexte.

Ces quations doivent tre formalises dune faon la plus linaire possiblepour rester le plus intgre possible. Aussi nous proposons de dfinir deux faonspossibles pour reprer un point 3D dans lespace visible de la camra :


FIGURE 2.4 Premire faon dobserver un point : coordonne cartsienne

en coordonnes cartsiennes : vue de la projection comme une coordonne(u, v).

en coordonnes sphriques : vue du point comme 2 angles(, ) partant ducentre optique vers le point observ.

Ces 2 faons de dcrire un point ont t dfinies parce quil nous semble per-tinent dutiliser des quations qui semblent plus linaires suivant tels ou tels para-mtres. Par exemple, on peut aisment penser quune variation de coordonne depixel dans une image amne une information plus pertinente en variation de coor-donne de localisationx, y, z. De mme il semble plus judicieux de mettre journotre tat en lacet-tangage par une mesure de variation dangle plutt que de pixel.Nous allons donc prsenter dans ce mmoire diffrentes faons de reprsenter unpoint dans un repre camra. Le choix de la reprsentation dun point fait encorelobjet de travaux. Dans ce mmoire nous exposerons brivement les reprsenta-tions.

2.4.1 Observation de points en coordonnes cartsiennes

La faon la plus vidente pour reprer un point dans lespacecamraest dutiliser ses coordonnes pixeliques. Ces travaux ontt montrs dans[Malartreet al., 2009a].

quation dobservationSoientRx(v), Ry(v) et Rz(v) les trois matrices de rotation lmentaires

respectivement suivant les axesx, y et z.

Rx(v) =

1 0 0

0 cos (v) sin (v)0 sin (v) cos ()


Ry(v) =

cos (v) 0 sin (v)0 1 0

sin (v) 0 cos ()

Rz(v) =

cos (v) sin (v) 0sin (v) cos (v) 0

0 0 1

(2.13)

Soit R(vm) = Ry(v).Rx(v).Rz(v) la matrice de rotation du repre v-hiculeRv vers le repre mondeRm. De mme, nous avons besoin deR(cv) =Ry(c).Rx(c).Rz(c) qui est la matrice de rotation du repre camraRc vers lerepre vhiculeRv et de ~T(cv) =

(

Tx Ty Tz)T

qui est le vecteur de transla-

tion de la camra vers le vhicule. Un point du monde~P =(

Px Py Pz)T

seraobserv dans le repre camra par la relation :

~Pc = KRT(cv)(R

T(vm)(

~Pn ~T(vm)) ~T(cv)) =(

xc yc zc)T

oK est la matrice des paramtres intrinsques de la camra :

K =

f.kx 0 Cx0 f.ky Cy0 0 1

=

~K1~K2~K3

avecf la distance focale (en mtres),ku et kv les facteurs dagrandissement delimage (en pixels/mtres) etCx et Cy les coordonnes du centre optique de lacamra (en pixels). Dans le cas dune camra perspective, nous avons :

u =xczc

and v =yczc

(2.14)

zc est dans ce cas laxe de projection (voir figure2.5). En gardant un formalismedes quations dobservations sous forme matricielle commepropos par Fraud[Fraudet al., 2010] et utilis dans [Malartreet al., 2009a] nous obtenons les qua-tions2.15et 2.16:

u =~K1R

T(cv)(R

T(vm)(

~Pn ~T(vm)) ~T(cv))~K3RT(cv)(R

T(vm)(

~Pn ~T(vm)) ~T(cv))=

NuDuv

(2.15)

v =~K2R

T(cv)(R

T(vm)(

~Pn ~T(vm)) ~T(cv))~K3R

T(cv)(R

T(vm)(

~Pn ~T(vm)) ~T(cv))=

NvDuv

(2.16)

O ~Ki reprsente la imeligne deK.Nous avons donc la relation qui relie lobservation dans le repre camra~yi =

(ui, vi)T du nmepoint ~Pn = (xi, yi, zi) avec le vecteur dtat

~X.


Z

Y

U

V

FIGURE 2.5 Observation dun amer dans la camra.

Ce qui implique :

~yi =

(

uivi

)

= h(~X) + ~i =

(

ui + uivi + vi

)

avec~i =

(

uivi

)

qui est le bruit de mesure dun pixel dans la camra et qui

sera considr comme un bruit blanc gaussien dcart type 1 pixel. Nous dis-posons prsent de la fonction dobservation de ltat dansla camra. Le filtrede Kalman tendu a besoin de la jacobienne de cette fonction pour projeter lesincertitudes de ltat dans lespace dobservation. La jacobienne dune fonctionquelconquef(x1, . . . , xn) est la matrice des drives partielles du premier ordre

Jf =(

f

x1. . . f

xn

)

.

Jacobienne de la positionLe filtre de Kalman tendu a besoin de la jacobienne de cette observation pour

mettre jour le vecteur dtat ainsi que la covariance. Prenons le cas du calcul dela jacobienne dobservation dun point quelquonqueP =

(

x y z)

. Cette jaco-bienne est reprsente dans lquation2.17.

Juv =

(

uXv

uYv

uZv

uv

uv

uv

... ux

uy

uz

vXv

vYv

vZv

vv

vv

vv

... vx

vy

vz

)

(2.17)

Nous avons donc besoin des quations dobservations (2.15) et (2.16) pour calculerces jacobiennes. Prenons le cas deu

Xv:

u

Xv=Duv

NuXv

Nu DuvXvD2uv

= KuRT(cv)R

T(vm)

10

0


avecKu = Duv~K1Nu ~K3D2uv

. Nous en dduisons alorsuY

et uZ

.

u

Yv= KuR

T(cv)R

T(vm)

0

10

u

Zv= KuR

T(cv)R

T(vm)

0

0

1

Les drives partiellesvXv

, vYv

et vZv

sont calcules de la mme faon avec

Kv =Duv ~K2Nv ~K3

D2uv.

Jacobienne de lorientationLe mme principe peut tre appliqu aux calculs des jacobiennes de lorienta-

tion. Reprenons lquation (2.15) pour lang

perception intelligente pour la navigation rapide de robots mobiles

Documents