Localisation et navigation de robots

Transcription

1 Localisation et navigation de robots UFR des Sciences, Département EEA M2 EEAII, parcours ViRob Semestre 9, 2014/2015 Fabio MORBIDI Laboratoire MIS Équipe Perception et Robotique Jeudi, salle TP102, h

2 2 Évolution du cours au fil du temps Localisation et navigation des robots (ViRob) Localisation, navigation et asservissement Vision avancée (ViRob) Asservissement visuel (Opt.)

3 3 Perspective d ensemble Perception Décision Environnement Action See-Think-Act

4 4 Exemple Perception: Il y a un mur en face! Décision: Tourne à gauche Action: Régle la vitesse des roues, ex. ω G (t) =0.5 rad/s, ω D (t) =0.1 rad/s, t [t a,t b ]

5 5 Deux questions cruciales Où suis-je?? Localisation! A? B Comment aller de A à B? Navigation! et aussi Comment détecter B? Comment éviter les obstacles?

6 6 Plan du cours Chapitre 1: Localisation 1.1 Introduction et défis 1.2 Odométrie 1.3 Filtrage et fusion de capteurs 1.4 Autres techniques de localisation Chapitre 2: Navigation 2.1 Stratégies de navigation 2.2 Architectures de contrôle 2.3 Navigation vers un but 2.4 Evitement d obstacles

7 7 Bibliographie Introduction to Autonomous Mobile Robots R. Siegwart, I.R. Nourbakhsh, D. Scaramuzza, MIT press, 2ème éd., 2011 (Chapitres 4, 5 et 6) Principles of Robot Motion: Theory, Algorithms, and Implementations H. Choset, K.M. Lynch, S. Hutchinson, G.A. Kantor, W. Burgard, L.E. Kavraki, S. Thrun, MIT press, 2005 Robotics: Modelling, Planning and Control B. Siciliano, L. Sciavicco, L. Villani, G. Oriolo, Springer, 1er éd., 2009 (Chapitres 4, 12) Estimation with Applications to Tracking and Navigation: Theory Algorithms and Software Y. Bar-Shalom, X.R. Li, T. Kirubarajan, John Wiley & Sons, 2001 (Chapitres 5, 6 et 12) Diapos du cours:

8 8 Avant de commencer Je suppose que vous avez une bonne connaissance: Algèbre linéaire (vecteurs, matrices, valeurs et vecteurs propres, etc.) Calcul multidimensionnel (gradients, matrice jacobienne, etc.) Théorie des probabilités, variables et processus aléatoires Théorie de l estimation (ex. filtre de Kalman) Notions de base de robotique à réviser dans les prochaines jours!!! Note finale: 50% CC, 50% TP

9 Localisation et navigation de robots UFR des Sciences, Département EEA M2 EEAII, parcours ViRob Chapitre 1 : Localisation F. Morbidi 2014/2015

10 10 Plan du chapitre Introduction et défis Partie 1 Odométrie Partie 2 Filtrage et fusion de capteurs Partie 3 Autres techniques de localisation Partie 4

11 Partie 1 : Introduction et défis 11

12 12 Introduction Localiser: Déterminer la place de quelque chose, l'endroit où se situe quelque chose (Dict. La Rousse) Vous êtes ici

13 13 Introduction Hypothèse: nous avons une carte de l environnement Facile Difficile

14 14 Introduction Robotique manipulatrice Robots sériels : où est l effecteur? Robot «fixe» L intégration et/ou la combinaison des angles articulaires permet(tent) de déduire la pose 3D de l effecteur Connaissance des longueurs des axes Plus un problème de calibrage que de localisation

15 15 Introduction Robotique manipulatrice Robots parallèles : où est l effecteur? Plus complexe que sériel Grande dynamique Plusieurs bases pour un même effecteur Mais zone/volume de mouvement limité et facilement identifiable

16 16 Introduction Robotique mobile Environnement ouvert Pas de lien fixe avec l environnement Pas de mesures directes de la pose (x, y, θ) Zone de mouvement «sans limite» Video ETHZ

17 17 Introduction Objectif : la navigation A grande échelle : besoin de localisation Compétence importante et difficile d un robot mobile Pour le succès de la navigation: Perception: le robot doit interpréter ses capteurs pour extraire des données pertinentes Localisation: le robot doit déterminer sa position dans l environnement Décision: le robot doit décider de comment agir pour atteindre son but Commande du mouvement: le robot doit actionner ses moteurs pour réaliser la trajectoire désirée

18 18 Introduction Localisation: il a reçu la plus grande attention de la recherche au cours de la dernière décennie

19 19 Où intervient la localisation? Connaissance, base de données Mission, commandes Localisation Construction de carte «position» carte globale Cognition, planif. de trajectoire modèle d environnement, carte locale trajectoire perception Extraction d information et interprétation données brutes Perception environnement, monde réel Réalisation de chemin commande d actionneurs Action contrôle de mouvement

20 20 Introduction Schéma général de localisation d un robot mobile Encoder base de données, carte prédiction de position (ex. odométrie) position position prédite mise à jour de position (estimation?) OK données brutes des capteurs ou primitives extraites appariement observation observations appariées perception

21 21 Les défis de la localisation Principalement Le bruit Perception (capteurs) Action (actionneurs) L aliasing Perception: l aliasing perceptuel définit le fait que deux lieux distincts peuvent avoir la même apparence

22 22 Les défis de la localisation GPS! Intuitivement «GPS» précis : solution ultime pour la localisation Informe le robot de sa position exacte Intérieur et extérieur Réponse immédiate à la question «où suis-je?» Mais un tel capteur n existe pas! GPS précis à quelques mètres Inadapté pour des environnements à taille humaine et encore moins pour les micro- et nano-robots GPS ne peut pas fonctionner à l intérieur ou dans environnement encombré (canyons urbains et gratte-ciel) GPS = Global Positioning System (constellation de 24 satellites orbitant à km)

23 23 Les défis de la localisation Au delà des limitations du GPS Localisation est plus que la connaissance de position absolue Exemple : robot interactif Éventuellement besoin d une position absolue Position relative aux humains aussi importante Sa tâche de localisation doit inclure: Perception (multi-capteurs) pour identifier les humains Calcul de sa position relative aux humains

24 24 Les défis de la localisation Atteindre un lieu particulier demande plus qu une «simple» localisation Étape de décision Sélection de stratégie pour atteindre des buts Besoin d acquérir ou de créer un modèle de son environnement : une carte (ex. SLAM) Aide le robot dans sa planification de trajectoire En résumé, la localisation c est plutôt Construire une carte de l environnement Identifier la position du robot relativement à la carte

25 25 Les défis de la localisation Éléments cruciaux de cette localisation Les capteurs du robot Ses actionneurs Les capteurs et actionneurs sont inexactes et incomplets Une mesure parfaite n existe pas Tout l environnement ne peut être mesuré d un coup

26 26 Le bruit des capteurs Perception Capteurs : fondamentaux Leur degré de discrimination de l environnement est donc critique Le bruit des capteurs Inconsistance de mesures dans un même environnement Source: Primitives de l environnement non perçues Négligence de ces primitives

27 27 Le bruit des capteurs Exemple 1 Système de vision pour localisation à l intérieur Utilisation des couleurs détectées par caméra CCD Soleil caché : illumination différente (fenêtres) Valeurs de teinte non constantes Couleurs CCD «bruitée» pour le robot Teinte inutilisable sauf si Connaissance de la position du soleil Connaissance des nuages Autres : flou (mise au point, bougé), gain du signal, etc.

28 28 Le bruit des capteurs Exemple 2 Sonar : télémètre à ultrasons Rapport signal sur bruit très pauvre Ultrasons émis vers une surface lisse, mal orientée Signal non reflété vers le capteur Pas d écho de retour Une petite quantité d énergie peut revenir malgré tout Si le seuil sur l amplitude d énergie reçue en est proche parfois le sonar permettra de détecter l objet parfois non Par conséquent : Pour le robot, un environnement inchangé peut donner deux lectures différentes pour un sonar : un long, un court! récepteur émetteur

29 29 Le bruit des capteurs Plusieurs sonars Interférences de l environnement entre émetteurs d ultrasons Sur les robots mobiles, en général Entre 12 et 48 sonars sur une même plateforme Dans certains environnements Interférences «multi-chemins» en émissions sonars Un signal émis par un sonar est reçu par un autre Peut engendrer d importantes erreurs de mesures (sousestimation): ils se produisent rarement, moins de 1% du temps En résumé Le bruit de perception réduit l information utile des mesures d un capteur La solution est de prendre en compte plusieurs mesures, en les fusionnant au cours du temps ou avec d autres capteurs afin d améliorer globalement l information perceptuelle du robot

30 30 L aliasing perceptuel Second inconvénient des capteurs Le contenu informationnel des mesures peut être très faible Exacerbe les problèmes de perception et de localisation Problème peu intuitif pour les humains On ne rencontre que rarement ce phénomène La perception humaine, particulièrement la vision, tend à percevoir des informations uniques dans chaque état local unique Chaque lieu différent semble différent Mais pas facile: Se déplacer dans un bâtiment inconnu qui est complètement sombre Se déplacer dans un labyrinthe à taille humaine (pas unicité visuelle)

31 L aliasing perceptuel En robotique L aliasing est la norme et non l exception! Exemple : sonars ou proximètres IR Champ de perception étroit Information de profondeur dans une seule direction Pas d info comme couleur, texture, rigidité de surface Même avec plusieurs de ces capteurs, plusieurs états dans l environnement donnent les mêmes mesures Formellement : Il y a une association N à 1 des états de l environnement vers la perception du robot Env. 1 Env Env. N

32 32 L aliasing perceptuel Exemple de situation concrète Distinguer les humains des objets Le robot détecte un obstacle face à lui Doit-il dire «excusez-moi» car l obstacle peut être un humain en mouvement? Ou le robot doit-il planifier un chemin autour de l objet car ce peut être un carton? Pour le(s) sonar(s) seul(s), ces deux états souffrent d aliasing et leur discrimination est impossible! humain ou objet?

33 33 L aliasing perceptuel L aliasing perceptuel pose le problème suivant Supposons des capteurs non bruités La quantité d information est trop faible pour identifier la position du robot à partir d un seul percept «Solution» Localisation basée capteurs et mesures multiples

34 34 Le bruit d actionnement Les difficultés de la localisation ne reposent pas uniquement sur les capteurs Les actionneurs sont aussi bruités À cause de la perception ou non Une même action ordonnée par le robot peut engendrer différentes réalisations même si du point de vue du robot, l état initial avec l action est parfaitement connu

35 35 Le bruit d actionnement Les actionneurs de robot mobile induisent une incertitude sur l état futur Pour un robot, se déplacer accroît son incertitude La couche décisionnelle peut minimiser cet effet Planification et/ou interprétation adaptée Retour des capteurs dans la boucle de commande

36 36 Le bruit d actionnement Nature du bruit en robotique mobile Du point de vue du robot Impossibilité d estimer sa propre position à partir de la connaissance de sa cinématique et de sa dynamique Vrai source d erreur Modèle incomplet de l environnement Type de surface non modélisé (rigidité, glissement ) Événements non envisagés (un humain pousse le robot)

37 37 Le bruit d actionnement Exemple : l odométrie Capteurs sur les roues seuls Éventuellement couplée à un gyroscope/compas («dead reckoning») Position basée capteurs proprioceptifs Mouvement intégré pour déduire la position Intégration des erreurs de mesure Accumulation d erreur en position au cours du temps Besoin d une autre localisation de temps en temps Autre mécanisme de localisation Sinon, la position estimée n a vite plus de sens L odométrie est une technique permettant d'estimer la position d'un véhicule en mouvement. Le terme vient du grec hodos ("voyage") et metron ("mesure")

38 38 Le bruit d actionnement Odométrie pure d un robot différentiel Sources d erreurs (environnement résolution): Résolution limitée pendant l intégration (incréments de temps, résolution de mesure, etc.) Mauvais alignement des roues (déterministe) Incertitude sur le diamètre de la roue et/ou diamètre non constant (déterministe) Variation du point de contact de la roue Contact variable avec le sol (glisse, bosse, sol mou, déformation)

39 39 L'expérience du chemin carré unidirectionnelle [Borenstein 1996]

40 40 Le bruit d actionnement Types de sources d erreurs Déterministes (systématiques) Résolues par calibrage du système Non-déterministes (aléatoires) Erreurs résiduelles Mènent à des incertitudes de position au fur et à mesure

41 41 Le bruit d actionnement Types géométriques d erreurs (robot différentiel) Erreur en distance Longueur du chemin intégré Somme des mouvements de roue Erreur en virage Similaire à l erreur en distance Différence des mouvement de roue Erreur de dérive (drift) Une différence d erreurs entre les roues mène à une erreur d orientation angulaire du robot

42 42 Le bruit d actionnement Sur de longues périodes Erreurs de virage et dérive plus importantes des erreurs en distance Contribution non-linéaire à l erreur de position Soit un robot de position initiale connue (x 0, y 0, θ 0 ) Il se déplace en ligne droite selon l axe x Erreur en y pour un mouvement de d mètres : dsin(δθ) Devient très importante à mesure que l erreur angulaire Δθ croît Au cours du temps, l erreur d orientation par rapport au repère d origine croît rapidement et donc, la position estimée aussi Il faut alors alors établir un modèle d erreur pour l exactitude odométrique et propager ces erreurs au cours du temps

43 43 Plan du chapitre Introduction et défis Partie 1 Odométrie Partie 2 Filtrage et fusion de capteurs Partie 3 Autres techniques de localisation Partie 4

44 Partie 2 : Odométrie 44

45 45 Notation a, λ, M u R n A R n n scalaires vecteur colonne de dimension n matrice avec lignes et colonnes n n I n R n n matrice identité n n 0 n m R n m matrice de zéros n m diag(a 1,a 2,...,a n )= a a matrice diagonale n n 0 0 a n

46 46 Modèle d erreur pour l exactitude odométrique Généralement la pose (position + orientation) d un robot mobile est répresentée par le vecteur: p =[x, y, θ] T Pour un, la position est estimée en partant d une position connue en intégrant le mouvement (somme des incréments des distances parcourues) robot de type unicycle v y ω θ Trajectoire du robot R m x

47 47 Modèle d erreur pour l exactitude odométrique Dans un système discret avec une période d échantillonnage fixée les incréments de distances parcourues sont: x = s cos(θ + θ/2) y = s sin(θ + θ/2) θ = s d s g L s = s d + s g 2 L t t +1 ( x, y, θ) s d, s g L Chemin parcouru pendant le dernier intervalle de temps Distances parcourues pour la roue droite et gauche, respectivement Distances entre les roues du robot

48 48 Modèle d erreur pour l exactitude odométrique La mise à jour de la pose en p = x y θ = p + p se fait par: s cos(θ + θ/2) s sin(θ + θ/2) θ En utilisant les relations pour s et θ vues précédemment, on obtient les équations de base de mise à jour de position odométrique pour un robot unicycle: s d + s g x 2 cos p = f(x, y, θ, s d, s g )= y + s d + s g 2 sin θ θ + s d s g 2L θ + s d s g 2L s d s g L

49 49 Modèle d erreur pour l exactitude odométrique Ces équations n ont d intérêt que pour de faibles déplacements Plus le déplacement entre deux instants sera important, plus l estimation de position sera grossière vis-à-vis de la réalité A cause des erreurs d intégration des incertitudes sur la pose et des erreurs de mouvement réalisées pendant l incrément ( s d, s g ) l erreur en position basée sur l intégration de l odométrie augmente avec le temps Etablissons un modèle d erreur pour la pose intégrée de covariance de l estimé odométrique de position Σ p p pour obtenir la matrice Pour ce faire, on va supposer la connaissance d une matrice de covariance Σ p R 3 3 initiale

50 Modèle d erreur pour l exactitude odométrique Pour l incrément de mouvement des roues ( s d, s g ) la matrice doit être connue aussi: où Σ =cov( s d, s g )= kd s d 0 0 k g s g Σ k d,k g sont des constantes représentant les parametrès non déterministes du moteur et de l interaction de la rue avec le sol Deux hypothèses sont englobées dans l équation précédente: Les erreurs de chaque roue sont indépendantes Les variances des erreurs sont proportionnelles à la valeur absolue des distances parcourues Ces hypothèses, bien qu imparfaits, sont satisfaisantes dans la plupart des cas en environnement structuré. Les sources d erreur sur le mouvement sont: roue déformée, glissement, sol non lisse, encodeurs imprécis, etc. Les valeurs des constants k d,k g dépendent du robot et de l environnement et ils sont déterminées expérimentalement 50

51 51 Modèle d erreur pour l exactitude odométrique p En admettant que et dg =[ s d, s g ] T ne sont pas corrélés et que la dérivée de f est bien approximée par la décomposition de Taylor du premier ordre (linéarisation), on peut appliquer la loi de propagation d erreur, obtenant: Σ p = p f Σ p ( p f) T + dg f Σ ( dg f) T Σ p Σ p La matrice est directement égale au précédent (et il peut être calculée en spécifiant une valeur initiale, ex. zéro) Connaissant on peut exprimer les deux jacobiens, : dg f = p f = f s d f s g f p f dg f 1 0 s sin(θ + θ/2) f f f = 0 1 s cos(θ + θ/2) x y θ = s 2 cos(θ + θ/2) 2L sin(θ + θ/2) 1 s 2 cos(θ + θ/2) + 2L sin(θ + θ/2) 1 s 2 sin(θ + θ/2) + 2L cos(θ + θ/2) 1 s 2 sin(θ + θ/2) 2L cos(θ + θ/2) 1/L 1/L

52 Modèle d erreur pour l exactitude odométrique Les figures ci-dessous montrent typiquement comment croissent les incertitudes sur la position au cours du temps (l'incertitude de l'orientation n'est pas représenté dans l'image, bien que son effet peut être observé indirectement) Les résultats ont été obtenu en appliquant le modèle de propagation d erreurs précédent et en représentant la matrice de covariance par une ellipse d incertitude (décomposition en valeurs propres de Σ p ) 52 Mouvement linéaire Mouvement circulaire (rayon const.)

53 53 Remarques L incertitude dans la direction perpendiculaire au mouvement croît plus vite que dans le sens du mouvement. Ce résulte de l intégration de l incertitude en orientation L axe principal de l ellipse d incertitude ne reste généralement pas exactement perpendiculaire à la direction du mouvement Mouvement linéaire Mouvement circulaire (rayon const.)

54 54 Calcul des ellipses d'incertitude La decomposition d une matrice carrée en valeurs et vecteurs propres est un outil de base que l on trouve dans de nombreuses disciplines (ex. Google PageRank) Définition Un vecteur non nul u R n est un vecteur propre d'une matrice si et seulement si il existe un scalaire tel que : λ Au = λ u u A R n n où est appelé valeur propre associée à. L'ensemble constitué des vecteurs propres de valeur propre λ, et du vecteur nul, est appelé le sous-espace propre de associé à la valeur propre u λ On détermine les valeurs et vecteurs propres en résolvant l équation: λ (A λ I n )u = 0 Remarque Les valeurs propres d'une matrice symétrique (c.-à.-d. A = A T ) sont réelles et tous ses vecteurs propres sont orthogonaux

55 55 Calcul des ellipses d'incertitude En définissant les deux matrices n n suivantes: où Λ = diag(λ 1, λ 2,...,λ n ), U =[u 1 u 2 u n ] λ 1, λ 2,...,λ n valeurs propres de la matrice décroissant A agencées par ordre u 1 vecteur propre associé à la valeur propre de forte (c.-à.-d. ) λ 1 A la plus on peut écrire: AU = U Λ

56 56 Calcul des ellipses d'incertitude Exemple: A = Valeurs propres ordonnées: , , Λ = U = Commande Matlab: [U, Lambda] = eig(a)

57 57 Calcul des ellipses d'incertitude Le façon commune de représenter l incertitude de localisation en deux dimensions (plan du sol) mais à trois degrés de liberté (position 2D et orientation) est l ellipse d incertitude autour de la position estimée La position réelle du robot, si les paramètres du modèle d erreur ont été bien choisis, doit se trouver dans cette ellipse En appliquant la décomposition en valeurs/vecteurs propres de les deux matrices et d'où: U Λ Σ p U = UΛ Σ p, on obtient u 1 λ 1 Σ p En prenant le vector propre associé à la plus forte valeur propre de on obtient l axe principale de l ellipsoïde 3D d incertitude Remarque Par définition, une matrice de covariance est une matrice symétrique et semi-définie positive (donc les valeurs propres sont réelles et non négatifs)

58 58 Calcul des ellipses d'incertitude u 1,x u 1,y Les coordonnées et du vecteur permettent de déduire l orientation de l ellipse dans le plan du sol: θ = arctan En pratique, on préfèrera l utilisation de la fonction, qui teste dans quel quart se trouve la position (u 1,x,u 1,y ) pour que θ ait un signe cohérent La longeur des demi-axes de l ellipse est définie par la racine carrée des deux valeurs propres de les plus fortes, c.-à.-d. et λ 1 λ 2 Cependant, et représentant en fait des variances. Par exemple, si les paramètres du modèle d erreur sont corrects, pour être sûr à 99.7% que la position réelle se situe dans l ellipse d incertitude, les demi-axes de celle-ci auront pour longeurs 3 λ 1 et 3 λ 2, respectivement u 1 u1,y u 1,x atan2 Σ p λ 1 λ 2

59 59 3 λ 2 3 λ 1 Cf. TD1 3σ u 1,y θ Ellipse d incertitude ( ) u 1,x N (µ, σ 2 ) x Si est une variable aléatoire qui suit la loi normale (ou gaussienne), µ est l espérance de la loi de probabilité, et est la variance: σ 2 Pr(µ σ x µ + σ) Pr(µ 2σ x µ +2σ) Pr(µ 3σ x µ +3σ) Pr(µ 5σ x µ +5σ)

60 60 Odométrie et les humains? Nous savons la longueur de nos jambes et l étendue du mouvement des nos articulations depuis notre adolescence La longueur des notres étapes est donc codée dans notre cerveau (bien que nous ne sommes pas complètement au courant) Nous pouvons estimer assez précisément la distance parcourue Même sans rétroaction visuelle (et sans compter les étapes), on a une bonne précision avec distances jusqu'à 27 m [Andre et Rogers, 2006]

61 61 Plan du chapitre Introduction et défis Partie 1 Odométrie Partie 2 Filtrage et fusion de capteurs Autres techniques de localisation Partie 3 Partie 4 2 octobre